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第 1 讲 ”预备 知识 
1.1 本 书 讲 什 么 


这 是 一 本 介绍 视觉 SLAM 的 书 ， 也 很 可 能 是 第 一 本 以 视觉 SLAM 为 主 
题 的 中 文书 。 

那么 ，SLAM 是 什么 ? 

SLAM 是 Simultaneous Localization and Mapping 的 缩写 ， 中 文 译作 “ 同 
时 定位 与 地 图 构建 "mH 。 它 是 指 搭载 特定 传感器 的 主体 ， 在 没有 环境 先 验 
信息 的 情况 下 ， 于 运动 过 程 中 建立 环境 的 模型 ， 同 时 估计 自己 的 运动 
。 如 果 这 里 的 传感器 主要 为 相机 ， 那 就 称 为 “视觉 SLAM”。 

本 书 的 主题 就 是 视觉 SLAM。 这 里 我 们 刻意 把 许多 个 定义 放 到 一 句 话 
中 ， 是 希望 读者 有 一 个 较 明 确 的 概念 。 首 先 ，SLAM 的 目的 是 解决 “定位 ” 
与 “地 图 构建 ”这 两 个 问题 。 也 就 是 说 ， 一 边 要 估计 传感器 自身 的 位 置 ， 
一 边 要 建立 周围 环境 的 模型 。 那 么 怎么 解决 呢 ? 这 需要 用 到 传感器 的 信 
息 。 传 感 器 能 以 一 定形 式 观 察 外 部 的 世界 ， 不 过 不 同 传感器 观察 的 方式 
是 不 同 的 。 而 之 所 以 要 化 一 本 书 的 内 容 去 讨论 这 个 问题 ， 是 因为 它 很 难 
一 一 特别 是 我 们 希望 实时 地 、 在 没有 先 验 知识 的 情况 下 进行 SLAM。 当 
用 相机 作为 传感器 时 ， 我 们 要 做 的 就 是 根据 一 张 张 连续 运动 的 图 像 (E 
们 形成 了 一 段 视 频 ) ， 从 中 推断 相机 的 运动 ， 以 及 周围 环境 的 情况 。 

这 似乎 是 个 很 直观 的 问题 。 我 们 上 自己 走 进 陌 生 的 环境 时 不 就 是 这 人 么 
做 的 吗 ? 


在 计算 机 视觉 (ComputerVision) 创立 之 初 ， 人 们 就 想象 着 有 朝 一 日 
计算 机 将 和 人 一 样 ， 通 过 眼睛 去 观察 世界 ， 理 解 周 遭 的 物体 ， 探 索 未 知 
的 领域 这 是 一 个 美妙 而 又 浪漫 的 梦想 ， 吸 引 了 无 数 的 科研 人 员 日 夜 
为 之 奋斗 中 。 我 们 曾经 以 为 这 件 事 情 并 不 困难 ， 然 而 进展 却 远 不 如 预想 
的 那么 顺利 。 我 们 眼中 的 花草 树木 、 虫 鱼 乌 兽 ， 在 计算 机 中 却 是 那样 的 
不 同 : 它们 只 是 一 个 个 由 数字 排列 而 成 的 矩阵 (Matrix) 。 让 计算 机 理解 
图 像 的 内 容 ， 就 像 让 我 们 自己 理解 这 些 数字 一 样 朵 难 。 我 们 既 不 了 解 自 
己 如 何 理解 图 像 ， 也 不 知道 计算 机 该 如 何 理解 、 探 索 这 个 世界 。 于 是 我 


们 困惑 了 很 信 ， 直 到 几 十 年 后 的 今天 ， 才 发 现 了 一 点 点 成 功 的 迹 销 : 通 
过 人 工 智 能 (Arti fi cial Intelligence) 和 机 器 学 习 (Machine Learning) 技 
A, 计算 机 渐渐 能 够 辨别 出 物体 、 人 上 脸 、 声 音 、 文 字 尽管 它 所 用 的 
方式 (概率 学 建 模 ) 与 我 们 是 如 此 不 同 。 另 一 方面 ， 在 SLAM 发 展 了 将 近 
30 年 之 后 ， 我 们 的 相机 才 渐 渐 开 始 能 够 认识 到 自身 的 位 置 ， 发 觉 目 己 在 
运动 虽然 方式 还 是 和 我 们 人 类 有 巨大 的 差异 。 不 过 ， 至 少 研究 者 们 
已 经 成 功 地 搭建 出 种 种 实时 SLAM 系 统 ， 有 的 能 够 快速 跟踪 自身 位 置 ， 有 
的 甚至 能 够 进行 实时 的 三 维 重 建 。 


这 件 事 情 确实 很 困难 ， 但 我 们 已 经 有 了 很 大 的 进展 。 更 令 人 兴奋 的 
是 ， 近 年 来 随 着 科技 的 发 展 ， 涌 现 出 了 一 大 批 与 SLAM 相 关 的 应 用 点 。 在 
许多 地 方 ， 我 们 都 希望 知道 自身 的 位 置 : 室内 的 扫地 机 和 移动 机 器 人 需 
要 定位 ， 野 外 的 自动 驾驶 汽车 需要 定位 ， 空 中 的 无 人 机 需要 定位 ， 虚 拟 
现实 和 增强 现实 的 设备 也 需要 定位 。SLAM 是 那样 重要 。 没 有 它 ， 扫 地 机 
就 无 法 在 房间 自主 地 移动 ， 只 能 盲 自 地 游荡 ;家 用 机 器 人 就 无 法 按照 指 
令 准确 到 达 某 个 房间 ; 虚拟 现实 也 将 永远 固定 在 座 椅 之 上 一 一 所 有 这 些 
新 奇 的 事物 都 无 法 出 现在 现实 生活 中 ， 那 将 多 么 令 人 遗憾 。 


今天 的 研究 者 和 应 用 开发 人 人员， 逐渐 意识 到 了 SLAM 技 术 的 重要 性 。 
在 国际 上 ，SLAM 已 经 有 近 三 十 年 的 研究 历史 ， 也 一 直 是 机 器 人 和 计算 机 
视觉 的 研究 热点 。21 世 纪 以 来 ， 以 视觉 传感器 为 中 心 的 视觉 SLAM 技 术 
， 在 理论 和 实践 上 都 经 历 了 明显 的 转变 与 突破 ， 正 逐步 从 实验 室 研究 迈 
向 市 场 应 用 。 同 时 ， 我 们 又 遗憾 地 发 现 ， 至 少 在 国内 ， 与 SLAM 相 关 的 论 
文 、 书 籍 仍然 非常 匮乏 ， 让 许多 对 SLAM 技 术 感 兴趣 的 初学 者 无 从 一 穿 门 
径 。 虽 然 SLAM 的 理论 框架 基本 趋 于 稳定 ， 但 其 编程 实现 仍然 较为 复杂 ， 
有 着 较 高 的 技术 门槛 。 刚 步 入 SLAM 领 域 的 研究 者 ， 不 得 不 花 很 长 的 时 
间 ， 学 习 大 量 的 知识 ， 往 往 要 走 过 许 多 弯路 才 得 以 接近 SLAM 技 术 的 核 
心 o 

本 书 全 面 系统 地 介绍 了 以 视觉 传感器 为 主体 的 视觉 SLAM 技 术 ， 我 们 
希望 它 能 (部 分 地 ) 填补 这 方面 资料 的 空白 。 我 们 会 详细 地 介绍 SLAM 的 
理论 背景 、 系 统 架 构 ， 以 及 各 个 模块 的 主流 做 法 。 同 时 ， 极 其 重视 实践 
: 本 书 介绍 的 所 有 重要 算法 ， 都 将 给 出 可 以 运行 的 实际 代码 ， 以 求 加 深 
读者 的 理解 。 之 所 以 这 么 做 ， 主 要 是 考虑 到 SLAM 毕 竟 是 一 项 和 实践 紧密 
相关 的 技术 。 再 漂亮 的 数学 理论 ， 如 果 不 能 转化 为 可 以 运行 的 代码 ， 那 
就 仍 是 可 望 而 不 可 即 的 空中 楼 阁 ， 没 有 实际 意义 。 我 们 相信 ， 实 践 出 真 


知 ， 实 践 出 真爱 。 只 有 实际 地 总 算 过 各 种 算法 之 后 ， 你 才能 真正 地 认识 
SLAM， 真 正 地 喜欢 上 科研 。 


自 1986 年 提出 以 来 和 ，SLAM 一 直 是 机 器 人 领域 的 热点 问题 。 关 于 它 
的 文献 数 以 千 计 ， 想 要 对 SLAM 发 展 史 上 的 所 有 算法 及 变种 做 一 个 完整 的 
说 明 ， 是 十 分 困难 而 且 没 有 必要 的 。 本 书 中 会 介绍 5LAM 所 牵涉 的 背景 知 
识 ， 例 如 射影 几何 、 计 算 机 视觉 、 状 态 估计 理论 、 李 和 群 李 代数 等 ， 并 在 
这 些 背 景 知识 之 上 ， 给 出 SLAM 这 棵 大 树 的 主干 ， 而 略 去 一 部 分 形状 奇 
特 、 纹 理 复杂 的 枝叶 。 我 们 认为 这 种 做 法 是 有 效 的 。 如 果 读 者 能 够 掌握 
主干 的 精髓 ， 那 么 自然 会 有 能 力 去 探索 那些 边缘 的 、 细 节 的 、 错 综 复杂 
的 前 沿 知 识 。 所 以 ， 我 们 的 目的 是 ， 让 SLAM 的 初学 者 通过 阅读 本 书 快速 
地 成 长 为 能 够 探索 这 个 领域 边缘 的 研究 者 。 另 一 方面 ， 即 便 你 已 经 是 
SLAM 领 域 的 研究 人 员 ， 本 书 也 可 能 有 一 些 你 还 觉得 陌生 的 地 方 ， 可 以 让 
你 产生 新 的 见解 。 

目前 ,与 SLAM 相 关 的 书籍 主要 有 《概率 机 器 人 》 (Probabilistic 
robotics) 回 、《 计 算 机 视觉 中 的 多 视图 几何 》 (Multiple View Geometry 
in Computer Vision) ©. 《机 器 人 学 中 的 状态 估计 》 (State Estimation 
for Robotics:A Matrix-Lie-Group Approach) ©". CARES., ERE 
面 、 推 导 严 并， 是 SLAM 研 究 者 中 脸 炙 人 口 的 经 典 教材 。 然 而 就 目前 来 
看 ， 还 存在 两 个 重要 的 问题 : 其 一 ， 这 些 图 书 的 目的 在 于 介绍 基础 理 
论 ，SLAM 只 是 其 应 用 之 一 。 因 此 ， 它 们 并 不 能 算是 专门 讲解 SLAM 的 书 
籍 。 其 二 ， 它 们 的 内 容 偏 重 于 数学 理论 ， 基 本 不 涉及 编程 实现 ， 导 致 读 
者 经 常 出 现 “ 书 能 看 懂 却 不 会 编程 ”的 情况 。 而 我 们 认为 ， 只 有 读者 杀 自 
实现 了 算法 ， 调 试 了 各 个 参数 ， 才 能 谈 得 上 真正 理解 了 问题 本 身 。 

我 们 会 提 及 SLAM 的 历史 、 理 论 、 算 法 、 现 状 ， 并 把 完整 的 SLAM 系 
统 分 成 几 个 模块 : 视觉 里 程 计 、 后 端 优化 、 建 图 ， 以 及 回环 检测 。 我 们 
将 陪 着 读者 一 点 点 实现 这 些 模块 中 的 核心 部 分 ， 探 讨 它们 在 什么 情况 下 
有 效 ， 什 么 情况 下 会 出 问题 ， 并 指导 大 家 在 自己 的 机 器 上 运行 这 些 代 
码 。 你 会 接触 到 一 些 必要 的 数学 理论 和 许多 编程 知识 ， 会 用 到 Figen、 
OpenCV、PCL、g20、Ceres 等 库 中 ， 掌 握 它 们 在 Linux 操 作 系统 中 的 使 用 
Fives 

从 写作 风格 上 ， 我 们 不 想 把 本 书写 成 枯燥 的 理论 书籍 。 技 术 类 图 书 
应 该 是 严 并 可靠 的 ， 但 严 并 不 意味 着 刻板 。 一 本 优秀 的 技术 书 应 该 是 生 
动 有 趣 而 易于 理解 的 。 如 果 你 觉得 “这 个 作者 怎么 这 么 不 正经 >， 冤 请 原 


这 ， 因 为 我 并 不 是 一 个 非常 严肃 的 人 中 。 无 论 如 何 ， 有 一 件 事 是 可 以 肯 
定 的 : 只 要 你 对 这 门 新 技术 感 兴趣 ， 在 学 习 本 书 的 过 程 中 肯定 会 有 所 收 
获 ! 您 会 掌握 与 SLAM 相 关 的 理论 知识 ， 你 的 编程 能 力也 将 有 明显 的 进 
步 。 在 很 多 时 候 ， 您 会 有 一 种 “我 在 陪 你 一 起 做 科研 ”的 感觉 ， 这 正 是 我 
所 希望 的 。 但 愿 您 能 在 此 过 程 中 发 现 研 究 的 乐趣 ， 喜 欢 这 种 “通过 一 番 努 
力 ， 看 到 事情 顺利 运行 ”的 成 就 感 。 


好 了 ， 话 不 多 说 ， 祝 你 旅行 愉快 ! 


1.2 ”如 何 使 用 本 书 
1.2.1 AAA 


本 书 名 为 “视觉 SLAM 十 四 讲 "。 顾 名 思 义 ， 我 们 会 像 在 学 校 里 讲课 那 
样 ， 以 * 讲 ”作为 本 书 的 基本 单元 。 每 一 讲 都 对 应 一 个 固定 的 主题 ， 其 中 
会 穿插 “理论 部 分 "和 “实践 部 分 ”两 种 内 容 。 通 常 是 理论 部 分 在 前 ， 实 践 部 
分 在 后 。 在 理论 部 分 中 ， 我 们 将 介绍 理解 算法 所 必需 的 数学 知识 ， 并 且 
大 多 数 时 候 以 叙述 的 方式 ， 而 不 是 像 数 学 教科 书 那 样 用 “定义 一 定理 一 推 
论 ” 的 方式 ， 因 为 我 们 觉得 这 样 的 方式 阅读 起 来 更 容易 一 些 ， 尽 管 有 时 候 
显得 不 那么 严谨。 实践 部 分 主要 是 编程 实现 ， 讨 论 程 序 里 各 部 分 的 含义 
及 实验 结果 。 看 到 标题 中 带 有 “实践 ”两 个 字 的 章节 ， 你 就 应 该 (KRHA 
Fhe) 打开 电脑 ， 和 我 们 一 起 愉快 地 编写 代码 了 。 


值得 一 提 的 是 ， 我 们 只 会 把 与 解决 问题 相关 的 数学 知识 放 在 书 里 ， 
并 尽量 保持 浅显 。 虽 然 我 们 是 工科 生 ， 但 也 要 承认 ， 有 某 些 做 法 只 要 经 验 
上 够 用 ， 没 必要 非得 在 数学 上 追求 完备 。 只 要 我 们 知道 这 些 算法 能 够 工 
作 ， 并 且 数 学 家 们 告诉 了 我 们 在 什么 情况 下 可 能 不 工作 ， 那 么 我 们 就 表 
示 满 意 ， 而 不 追究 那些 看 似 完美 但 实际 复杂 见长 的 证 明 (当然 它们 固有 
自己 的 价值 ) 。 由 于 SLAM 牵 涉 到 了 太 多 数学 背景 ， 为 了 防止 使 本 书 变 成 
数学 教科 书 ， 我 们 把 一 些 细节 上 的 推导 和 证 明 留 作 习 题 和 补充 阅读 材 
料 ， 方便 感 兴趣 的 读者 进一步 阅读 参考 文献 ， 更 深入 地 掌握 相关 细节 。 

每 一 讲 正 文 之 后 ， 我 们 设计 了 一 些 习 题 。 其 中 ， 带 * 号 的 习题 是 具有 
一 定 难 度 的。 我们 强烈 建议 读者 把 习题 都 练习 一 遍 ， 这 对 你 掌握 这 些 知 
识 很 有 帮助 。 


全 书 内 容 主要 分 为 两 个 部 分 。 

1. 第 一 部 分 为 数学 基础 篇 ， 我 们 会 以 浅显 易 懂 的 方式 ， 铺 垫 与 视觉 
SLAM 相 关 的 

数学 知识 ， 包 括 : 

“第 1 讲 是 前 言 ， 介 绍 这 本 书 的 基本 信息 ， 习 题 部 分 主要 包括 一 些 自 测 
题 。 

第 2 讲 为 SLAM 系 统 概 述 ， 介 绍 一 个 SLAM 系 统 由 哪些 模块 组 成 ， 各 
模块 的 具体 工作 是 什么 。 实 践 部 分 介绍 编程 环境 的 搭建 过 程 以 及 IDE 的 使 
用 。 

:第 3 讲 介绍 三 维 空间 运动 ， 你 将 接触 到 旋转 矩阵、 四 元 数 、 欧 拉 角 的 
相关 知识 ， 并 且 在 Eigen 当 中 使 用 它们 。 

:第 4 讲 为 李 群 和 李 代 数 。 即 便 你 现在 不 懂 李 代数 为 何 物 ， 也 没有 关 
系 。 你 将 学 到 李 代 数 的 定义 和 使 用 方式 ， 然 后 通过 Sophus 操 作 它 们 。 

“第 5 讲 介 绍 针 孔 相 机 模型 以 及 图 像 在 计算 机 中 的 表达 。 你 将 用 
OpenCV 来 调 取 相机 的 内 外 参数 。 


“第 6 讲 介绍 非 线 性 优化 ， 包 括 状 态 估计 理论 基础 、 最 小 二 乘 问题 、 梯 
度 下 降 方 法 。 你 会 完成 一 个 使 用 Ceres 和 8g2o 进 行 曲 线 拟 合 的 实验 。 


这 些 就 是 我 们 要 用 到 的 所 有 数学 知识 了 ， 当 然 ， 其 中 还 隐 含 了 你 以 
前 学 过 的 高 等 数学 和 线性 代数 。 我 们 保证 它们 看 起 来 都 不 会 很 难 。 当 
然 ， 若 你 想 进 一 步 深 入 挖掘 ， 我 们 会 提供 一 些 参考 资料 供 你 阅读 ， 那 些 
材料 可 能 会 比 正文 里 讲 的 知识 难 一 些 。 

2. 第 二 部 分 为 SLAM 技 术 篇 。 我 们 会 使 用 第 一 部 分 所 介绍 的 理论 ， 讲 
述 视 觉 SLAM 中 各 个 模块 的 工作 原理 。 

第 7 讲 为 特征 点 法 的 视觉 里 程 计 。 该 讲 内 容 比 较 多 ， 包 括 特 征 点 的 提 
取 与 匹配 、 对 极 几 何 约束 的 计算 、PnP 和 ICP 等 。 在 实践 中 ， 你 将 用 这 些 
方法 去 估计 两 个 图 像 之 间 的 运动 。 

“第 8 讲 为 直接 法 的 视觉 里 程 计 。 你 将 学 习 光 流 和 直接 法 的 原理 ， 然 后 
利用 g2o 实 现 一 个 简单 的 RGB-D 直 接 法 。 

“第 9 讲 为 视觉 里 程 计 的 实践 章 ， 你 将 搭建 一 个 视觉 里 程 计 框架 ， 综 合 
运用 先前 学 过 的 知识 ， 实 现 它 的 基本 功能 。 这 个 过 程 中 ， 你 会 碰 到 一 些 


问题 ， 例 如 优化 的 必要 性 、 天 键 帧 的 选择 等 。 


。 第 10 讲 为 后 端 优化 ， 主 要 为 对 Bundle Adjustment 的 深入 讨论 ， 包 括 
基本 的 BA， 以 及 如 何 利用 稀 玻 性 加 速 求 解 过 程 。 你 将 用 Ceres 和 8g2o 分 别 
书写 一 个 BA 程序 。 


“第 11 讲 主要 讲 后 端 优化 中 的 位 次 图 。 位 次 图 是 表达 关键 帧 之 间 约束 
的 一 种 更 紧凑 的 形式 。 你 将 用 g2o 和 gtsam 对 一 个 位 姿 球 进行 优化 。 


"第 12 讲 为 回环 检测 ， 主 要 介绍 以 词 袋 方法 为 主 的 回环 检测 。 你 将 使 
用 dbow3 书 写字 典 训练 程序 和 回环 检测 程序 。 


“第 13 讲 为 地 图 构建 。 我 们 会 讨论 如 何 使 用 单 目 进行 稠密 深度 图 的 估 
计 〈 以 及 这 是 多 么 不 可 靠 ) ， 然 后 讨论 RGB-D 的 稠密 地 图 构建 过 程 。 你 
会 书写 极 线 搜索 与 块 匹 配 的 程序 ， 然 后 在 RGB-D 中 遇 到 点 云 地 图 和 八 叉 
树 地 图 的 构建 问题 。 


“第 14 讲 主要 介绍 当前 的 开源 SLAM 项 目 以 及 未 来 的 发 展 方 向 。 相 信 
在 阅读 了 前 面 的 知识 之 后 ， 你 会 更 容易 理解 它们 的 原理 ， 实 现 目 己 的 新 
想法 。 


最 后 ， 如 果 你 完全 看 不 懂 上 面 在 说 什么 ， 那 么 恭喜 你 ! 这 本 书 很 适 
合 你 ! 加 油 ! 


1.2.2 ”代码 


本 书 所 有 源 代 码 均 托管 在 github 上 : 
https://github.com/gaoxiang12/slambook 


强烈 建议 读者 下 载 它们 以 供 随时 查看 。 代 码 是 按 章节 划分 的 ， 比 
如 ， 第 7 讲 的 内 容 就 会 放 在 ch7 文 件 夹 中 。 上 此外， 对 于 书 中 用 到 的 一 些小 
型 库 ， 会 以 压缩 包 的 形式 放 在 3rdparty 文 件 夹 下 。 对 于 像 OpenCV 那 种 大 
中 型 库 ， 我 们 会 在 它们 第 一 次 出 现时 介绍 其 安装 方法 。 如 果 你 对 代码 有 
任何 疑问 ， 请 单 击 GitHub 上 的 Issues 按 钮 ， 提 交 问 题 。 如 果 确 实 是 代码 出 
现 问题 ， 我 们 会 及 时 进行 修改 ， 即 使 是 你 的 理解 有 偏差 ， 我们 也 会 尽 可 
能 回复 。 如 果 你 不 习惯 使 用 Git， 那 么 单 击 右 侧 包 含 download 字 样 的 按钮 
将 代码 下 载 至 本 地 即 可 。 


1.2.3 面向 的 读者 


本 书面 向 对 SLAM 感 兴趣 的 学 生 和 研究 人 员 。 阅 读本 书 需要 一 定 的 基 
础 ， 我 们 假设 你 具备 以 下 知识 : 


“高 等 数学 、 线 性 代数 、 概 率 论 。 这 些 是 大 部 分 读者 应 该 在 大 学 本 科 
阶段 接触 过 的 基本 数学 知识 。 你 应 当 明 白 和 矩阵 和 向 量 是 什么 ， 或 者 做 微 
分 和 积分 是 什么 意思 。 对 于 SLAM 中 用 到 的 专业 知识 ， 我 们 会 额外 加 以 介 
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.C++ 语言 基础 。 由 于 我 们 采用 C++ 作为 编码 语言 ， 所 以 建议 读者 至 
少 熟悉 这 门 语言 的 语法 。 比 如 ， 你 应 该 知道 类 是 什么 ， 如 何 使 用 C++ 标准 
库 ， 模 板 类 如 何 使 用 ， 等 等 。 我 们 会 避免 过 多 地 使 用 技巧 ， 但 有 些 地 方 
确实 无 法 避免 。 此 外 ， 我 们 还 使 用 了 一 些 C++11 标 准 的 内 容 ， 不 过 ， 我 们 
会 在 用 到 的 地 方 加 以 解释 。 


Linux 基 础 。 我 们 的 开发 环境 是 Linux 而 非 Windows， 并 且 只 提供 
Linux 下 的 源 程 序 ， 不 会 再 提供 Windows 下 的 开发 方法 介绍 。 我 们 认为 ， 
掌握 Linux 是 一 个 SLAM 人 研究 人 员 所 必需 的 ， 请 初学 者 暂时 不 要 问 为 什 
么 ， 把 本 书 的 知识 学 好 之 后 相信 你 会 和 我 们 有 同样 的 想法 。 各 种 程序 库 
在 Linux 下 的 配置 都 非常 便捷 ， 你 也 会 在 此 过 程 中 体会 到 Linux 的 便利 。 如 
果 读 者 此 前 从 未 使 用 过 Linux， 那 么 最 好 找 一 本 Linux 的 教材 稍 加 学 习 (= 
握 基 本 知识 即 可 ， 一 般 就 是 相关 图 书 的 前 面 几 章 内 容 ) 。 我 们 不 要 求 读 
者 具备 高 超 的 Linux 操 作 技 能 ， 但 希望 读者 至 少 知道 “打开 终端 ， 进 入 代码 
目录 ”是 如 何 操作 的 。 本 讲 的 习题 里 有 一 些 Linux 知 识 自 测 题 ， 如 果 你 清楚 
自 测 题 的 答案 ， 那 么 阅读 本 书 代码 不 会 有 任何 问题 。 


对 SLAM 感 兴趣 但 不 具备 上 述 知 识 的 读者 ， 可 能 在 阅读 本 书 时 会 感到 
困难 。 如 果 你 不 了 解 C++ 的 基本 知识 ， 可 以 读 一 点 C++Primer Plus 之 类 
的 图 书 入 门 ; 如 果 你 缺少 相关 的 数学 知识 ， 也 可 以 先 阅读 一 些 相 关 数 学 
教材 补充 知识 ， 不 过 我 们 认为 ， 对 大 多 数 大 学 本 科 水 平 的 朋友 ， 读 懂 本 
书 所 需 的 数学 背景 肯定 是 具备 了 。 代 人 码 方面 ， 你 最 好 论点 时 间 亲 自 输入 
一 遍 ， 再 调节 里 面 的 参数 ， 看 看 效果 会 发 生 怎 样 的 改变 。 这 会 对 学 习 很 
有 帮助 。 


本 书 可 作为 SLAM 相 关 课 程 的 教材 ， 亦 可 作为 课外 上 自学 材料 使 用 。 


1.3 风格 约定 


本 书 既 有 数学 理论 介绍 ， 也 有 编程 实现 ， 因 此 ， 为 方便 阅读 ， 对 不 
同 内 容 采 用 了 不 同 排版 方式 加 以 区 分 。 

1. 数 学 公式 单独 列 出 ， 重 要 的 公式 还 在 右 侧 标 了 序号 ， 例 如 : 

y = Az. (1.1) 

标量 使 用 斜体 字 (如 a ) ， 向 量 和 和 矩阵 使 用 粗 斜 体 (aA). Sd 
粗 体 代表 特殊 集合 ， 如 实数 集 R、 整 数 集 Z。 李 代数 部 分 使 用 哥 特 体 ， 如 
se(3)o 

2. 程 序 代码 以 方 框框 出 ， 使 用 小 一 些 的 字号 ， 左 侧 带 有 行 号 。 如 果 程 
序 较 长 ， 方 框 会 延续 到 下 一 页 : 


1 |#include <iostream> 


2 |using namespace std; 


4 |int main ( int argc, char** argv ) 
s |{ 
cout<<"Hello"<<end1; 


return 0; 


8 |} 


3. 当 代码 数量 较 多 或 有 的 部 分 与 之 前 列 出 的 重复 ， 不 适合 完全 列 在 书 
中 时 ， 我 们 会 仅 给 出 重要 片段 ， 并 以 “片段 ”二 字 注 明 。 因 此 ， 我 们 强烈 
建议 读者 到 GitHub 上 下 载 所 有 源 代 码 ， 完 成 练习 ， 以 更 好 地 掌握 本 书 知 
识 。 

4. 由 于 排版 原因 ， 书 中 展示 的 代码 可 能 与 GitHub 中 的 代码 有 稍 许 不 
同 ， 请 以 GitHub 上 的 代码 为 准 。 

5. 我 们 用 到 的 每 个 库 ， 在 第 一 次 出 现 的 时 候 会 有 比较 详细 的 说 明 ， 但 
在 后 续 的 使 用 中 则 不 再 痪 述 。 所 以 ， 建 议 读 者 按 章节 顺序 阅读 本 书 内 
容 。 

6. 每 一 讲 的 开头 会 列 出 本 讲 的 内 容 提 要 ， 而 末尾 会 有 小 结 和 练习 题 。 
引用 的 参考 文献 在 全 书 末尾 列 出 。 


7. 以 星 号 开头 的 章节 是 选读 部 分 ， 读 者 可 以 根据 兴趣 阅读 。 跳 过 它们 
会 对 理解 后 续 章 节 产 生 影 响 。 


8. 文 中 重要 的 内 容 以 黑体 标 出 ， 相 信 你 已 经 习惯 了 。 

9. 我 们 设计 的 实验 大 多 数 是 演示 性 质 的 。 看 懂 了 它们 不 代表 你 已 经 熟 
悉 整 个 库 的 使 用 。 所 以 我 们 建议 你 在 课外 花 一 点 时 间 ， 对 本 书 经 常用 的 
几 个 库 进行 深入 学 习 。 

10. 本 书 的 习题 和 选读 内 容 可 能 需要 你 自己 搜索 额外 材料 ， 所 以 你 需 
要 学 会 使 用 搜索 引擎 。 


s = 
1.4 BONAR 
在 本 书 漫 长 的 写作 过 程 中 ， 我 们 得 到 了 许多 人 的 帮助 ， 包 括 但 不 限 
F: 


中科院 的 咒 一 家 博士 为 第 5 讲 的 相机 模型 部 分 提供 了 材料 。 

“ 颜 沁 窘 提供 了 第 7 讲 的 公 陈 推导 材料 。 

“华中 科大 的 刘 毅 博士 为 本 书 第 6 讲 和 第 10 讲 提供 了 材料 。 

“众多 的 老师 、 同 学 为 本 书 提供 了 修改 意见 : 肖 锡 至 、 谢 晓 佳 、 耿 
R FIIR UER RA, ASRA, RRA, ER, RR TX 
A SET. MEN, Hw. BD, RE. SRA K, AM. A 
美 奇 、 杨 楠 ， 等 等 。 在 此 向 他 们 表示 感谢 。 

此 外 ， 感 谢 我 的 导师 张涛 教授 一 直 以 来 对 我 的 支持 和 帮助 。 感 谢 电 
子 工业 出 版 社 郊 柳 洁 编辑 的 支持 。 疫 有 他 们 的 帮助 ， 本 书 不 可 能 以 现在 
的 面貌 来 到 读者 面前 。 本 书 的 成 书 与 出 版 是 所 有 人 共同 努力 的 结晶 ， 尽 
管 我 没 法 把 他 们 都 列 在 作者 列表 中 ， 但 是 它 的 出 版 离 不 开 他 们 的 工作 。 


本 书写 作 过 程 中 参考 了 大 量 文 献 和 论文 。 其 中 大 部 分 数学 理论 知识 
是 前 人 研究 的 成 果 ， 并 非 我 的 原创 。 一 小 部 分 实验 设计 亦 来 自 各 开源 代 
码 的 演示 程序 ， 不 过 大 部 分 是 我 自己 编写 的 。 此 外 ， 也 有 一 些 图 片 摘 目 
公开 发 表 的 期 刊 或 会 议论 文 ， 文 中 均 已 注 明 。 未 做 说 明 的 图 像 ， 或 为 原 
创 ， 或 来 自 网 络 ， 恕 不 一 一 列举 。 如 有 问题 ， 请 与 我 们 联系 ， 我 们 会 在 
第 一 时 间 加 以 修正 。 


本 书 涉 及 知识 点 众多 ， 错 漏 在 所 难免 。 如 有 疑问 ， 欢 迎 通过 电子 邮 
件 与 我 联系 。 


我 的 邮箱 是 : gaoxiang12@mails.tsinghua.edu.cno 

感谢 我 的 爱人 刘 丽 莲 女 士 长 期 的 理解 和 支持 。 这 本 书 是 献 给 她 的 。 

习题 (基本 知识 自 测 题 ) 

1. 有 线性 方程 Ax =b ， 若 已 知 A,b ， 需 要 求解 x ， 该 如 何 求解 ? 这 对 A 
和 有 哪些 要 求 ? tem: 从 A 的 维度 和 秩 角度 来 分 析 。 

2. 高 斯 分 布 是 什么 ? 它 的 一 维 形式 是 什么 样子 ? 它 的 高 维 形式 是 什么 
样子 ? 

3. 你 知道 C++ 中 的 类 吗 ? 你 知道 STL 吗 ? 你 使 用 过 它们 吗 ? 

4. 你 以 前 怎样 书写 C++ 程序 ? (你 完全 可 以 说 只 在 Visual C++6.0 下 与 
过 C++ 工程 ， 只 要 你 有 写 C++ 和 C 语 言 的 经 验 就 行 。) 

5. 你 知道 C++11 标 准 吗 ? 其 中 哪些 新 特性 你 听 说 过 或 用 过 ? 有 没有 其 
他 的 标准 ? 

6. 你 知道 Linux 吗 ? 你 有 没有 至 少 使 用 过 一 种 〈 不 算 安 卓 ) ， 比 如 
Ubuntu? 

7.Linux 的 目录 结构 是 什么 样 的 ? 你 知道 哪些 基本 命令 ， 比 如 1s,cat 
=? 

8. 如 何在 Ubuntu 中 安装 软件 〈 不 打开 软件 中 心 的 情况 下 ) ? 这 些 软件 
被 安装 在 什么 地 方 ? 如 果 只 知道 模糊 的 软件 名 称 (比如 想 要 装 一 个 名 称 
中 含有 eigen 的 库 ) ， 应 该 如 何 安装 它 ? 

9.* 花 一 个 小 时 学 习 一 下 Vim， 因 为 你 迟早 会 用 它 。 你 可 以 在 终端 中 
输入 vimtutor 阅 读 一 遍 所 有 内 容 。 我 们 不 需要 你 非常 熟练 地 操作 它 ， 只 要 
能 够 在 学 习 本 书 的 过 程 中 使 用 它 输入 代码 即 可 。 不 要 在 它 的 插件 上 浪费 
时 间 ， 不 要 想 着 把 vim 用 成 IDE， 我 们 只 用 它 做 文本 编辑 的 工作 。 


[1] 如 果 你 完全 没有 听 说 过 它们 ， 那 么 应 该 感到 兴奋 ， 这 说 明 你 会 从 本 书 中 收获 很 多 知识 。 
[2] 你 会 经 常 在 脚注 中 发 现 一 些 神奇 的 东西 。 
[3] 它们 也 可 能 成 为 今后 相关 行业 的 面试 题 ， 或 许 还 能 帮 你 在 找 工作 时 留 个 好 印象 。 


第 2 讲 ” 初 识 SLAM 


主要 目标 

1. 理 解 一 个 视觉 SLAM 框 架 由 哪 几 个 模块 组 成 ， 各 模块 的 任务 是 什 
pg 
Zo 

2. 搭 建 编 程 环境 ， 为 开发 和 实验 做 准备 。 

3. 理 解 如 何在 Linux 下 编译 并 运行 一 个 程序 ， 如 果 程 序 出 了 问题 ， 又 
该 如 何 对 它 进行 调试 。 

4. 掌 握 cmake 的 基本 使 用 方法 。 

本 讲 概括 地 介绍 一 个 视觉 SLAM 系 统 的 结构 ， 作 为 后 续 内 容 的 大 纲 。 
实践 部 分 介绍 环境 搭建 、 程 序 基本 知识 ， 最 后 完成 一 个 “Hello SLAM” 程 
序 。 
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2.1 引子 : 小 萝卜 的 例子 


假设 我 们 组 闭 了 一 台 叫 作 “ 小 萝卜 ”的 机 器 人 ， 大 概 的 样子 如 图 2-1 所 


小 o 


发 射 信息 的 天 线 


一 组 轮子 


图 2-1 小 萝卜 设计 图 。 左 边 : 正视 图 ; Ai: _ 侧 视图 。 设备 有 相机 、 轮 子 、 笔 记 本 ， 手 是 

虽然 有 点 像 *“ 安 卓 "， 但 它 并 不 是 靠 安 卓 系统 来 计算 的 。 我 们 把 一 台 
笔记 本 塞 进 了 它 的 后 备 箱 内 (方便 我 们 随时 拿 出 来 调试 程序 ) 。 它 能 做 
点 什么 呢 ? 

我 们 希望 小 萝卜 具有 自主 运动 能 力 。 虽 然 世界 上 也 有 放 在 桌面 像 摆 
件 一 样 的 机 器 人 ， 能 够 和 人 说 话 或 播放 音乐 ， 不 过 一 台 平 板 电 脑 完全 可 
以 胜任 这 些 事情 。 作 为 机 器 人 ， 我 们 希望 小 萝卜 能 够 在 房间 里 自由 移 
动 。 不 管 我 们 在 哪里 招呼 一 声 ， 它 都 会 滴 溜 溜 地 走 过 来 。 

要 移动 首先 得 有 轮子 和 电机 ， 所 以 我 们 在 小 萝卜 的 下 方 安装 了 轮子 
〈 足 式 机 器 人 步 态 很 复杂 ， 我 们 暂时 不 考虑 ) 。 有 了 轮子 ， 机 器 人 就 能 
够 四 处 行动 了 ， 但 不 加 控制 的 话 ， 小 萝卜 不 知道 行动 的 目标 ， 就 只 能 四 
处 乱 走 ， 更 糟糕 的 情况 下 会 撞 上 墙 造成 损毁 。 为 了 避免 这 种 情况 的 发 
生 ， 我 们 在 它 的 脑袋 上 安装 了 一 个 相机 。 安 装 相 机 的 主要 动机 ， 是 考虑 
到 这 样 一 个 机 器 人 和 人 类 非常 相似 从 画面 上 一 眼 就 能 看 出 。 有 有 眼 


青 、 大 脑 和 四 肢 的 人 类 ， 能 够 在 任意 环境 里 轻松 自在 地 行走 、 探 索 ， 我 
i] (RAH) 觉得 机 器 人 也 能 够 完成 这 件 事 。 为 了 使 小 萝卜 能 够 探索 一 
个 房间 ， 它 至 少 需要 知道 两 件 事 : 


1. 我 在 什么 地 方 ? 一 一 定位 。 
2. 周 围 环境 是 什么 样 ? 一 一 建 图 。 


“定位 "和 “ 建 图 ”， 可 以 看 成 感知 的 “内 外 之 分 ”。 作为 一 个 “内 外 兼 修 ” 
的 小 萝卜 ， 一 方面 要 明白 自身 的 状态 (即位 置 ) ， 另 一 方面 也 要 了 解 外 
在 的 环境 〈 即 地 图 ) 。 当 然 ， 解 决 这 两 个 问题 的 方法 非常 多 。 比 方 说 ， 
我 们 可 以 在 房间 地 板 上 铺设 导 引 线 ， 在 墙壁 上 贴 识 别 二 维 码 ， 在 桌子 上 
放置 无 线 电 定位 设备 。 如 果 在 室外 ， 还 可 以 在 小 萝卜 脑袋 上 安装 定位 设 
备 〈 像 手机 或 汽车 一 样 ) 。 有 了 这 些 东西 之 后 ， 定 位 问题 是 否 已 经 解决 
SME? 我 们 不 妨 把 这 些 传感器 〈 见 图 2-2) 分 为 两 类 。 


一 类 传感器 是 携带 于 机 器 人 本 体 上 的 ， 例 如 机 器 人 的 轮 式 编码 器 、 
相机 、 激 光 传 感 器 ， 等 等 。 另 一 类 是 安装 于 环境 中 的 ， 例 如 前 面 讲 的 导 
轨 、 二 维 码 标志 ， 等 等 。 安 装 于 环境 中 的 传 感 设 备 ， 通 党 能 够 直接 测量 
到 机 器 人 的 位 置信 息 ， 简 单 有 效 地 解决 定位 问题 。 然 而 ， 由 于 它们 必须 
在 环境 中 设置 ， 在 一 定 程度 上 限制 了 机 器 人 的 使 用 汇 围 。 比 方 说 ， 有 些 
地 方 没有 GPS 信号 ， 有 些 地 方 无 法 铺设 导轨 ， 这 时 该 怎么 做 定位 呢 ? 


(c) 


(b) 


图 2-2 一 些 传感器 。 (a) 利用 二 维 码 进行 定位 的 增强 现实 软件 ; (b) GPS 定位 装置 ; 
(c) 铺设 导轨 的 小 车 ; (d 激光 雷达 ; (e) IMU 单 元 ; (f) 双 目 相机 。 


(f) 


我 们 看 到 ， 这 类 传感器 约束 了 外 部 环境 。 只 有 在 这 些 约束 满足 时 ， 
基于 它们 的 定位 方案 才能 工作 。 反 之 ， 当 约束 无 法 满足 时 ， 我 们 就 没 法 
进行 定位 了 。 所 以 说 ， 虽 然 这 类 传感器 简单 可 靠 ， 但 它们 无 法 提供 一 个 
普遍 的 、 通 用 的 解决 方案 。 相 对 地 ， 那 些 携 带 于 机 器 人 本 体 上 的 传 感 
器 ， 比 如 激光 传感器 、 相 机 、 轮 式 编 码 器 、 惯 性 测量 单元 (Inertial 
Measurement Unit, IMU) 等 ， 它 们 测 到 的 通常 都 是 一 些 间接 的 物理 量 而 
不 是 直接 的 位 置 数 据 。 例 如 ， 轮 式 编码 器 会 测 到 轮子 转动 的 角度 ，IMU 
测量 运动 的 角速度 和 加 速度 ， 相 机 和 激光 传感器 则 读 取 外 部 环境 的 某 种 
观测 数据 。 我 们 只 能 通过 一 些 间 接 的 手段 ， 从 这 些 数据 推算 自己 的 位 
置 。 昌 然 这 听 上 去 是 一 种 迁 回 战术 ， 但 更 明显 的 好 处 是 ， 它 没有 对 环境 
提出 任何 要 求 ， 从 而 使 得 这 种 定位 方案 可 适用 于 未 知 环境 。 

回顾 前 面 讨 论 过 的 SLAM 定 义 ， 我 们 在 SLAM 中 非常 强调 未 知 环境 。 
在 理论 上 上， 我们 没 法 限制 小 萝卜 的 使 用 环境 上 ， 这 意味 着 我 们 没 法 假设 
像 GPS 这 些 外 部 传感器 都 能 顺利 工作 。 因 此 ， 使 用 携带 式 的 传感器 来 完成 
SLAM 是 我 们 重点 关心 的 问题 。 特 别 地 ， 当 谈论 视觉 SLAM 时 ， 我 们 主要 
是 指 如 何 用 相机 解决 定位 和 建 图 问题 。 


视觉 SLAM 是 本 书 的 主题 ， 所 以 我 们 尤其 关心 小 萝卜 的 眼睛 能 够 做 些 
什么 事 。SLAM 中 使 用 的 相机 与 我 们 平时 见 到 的 单反 摄像 头 并 不 是 同一 个 
东西 。 它 往往 更 加 简单 ， 不 携带 昂贵 的 镜头 ， 而 是 以 一 定 速率 拍摄 周围 
的 环境 ， 形 成 一 个 连续 的 视频 流 。 普 通 的 摄像 头 能 以 每 秒 钟 30 张 图 片 的 
速度 来 集 图 像 ， 高 速 相机 则 更 快 一 些 。 按 照 工 作 方 式 的 不 同 ， 相 机 可 以 
分 为 单 目 相机 (Monocular) 、 双 目 相 机 (Stereo) 和 深度 相机 (RGB- 
D) 三 大 类 ， 如 图 2-3 所 示 。 直 观看 来 ， 单 目 相 机 只 有 一 个 摄像 头 ， 双 目 
有 两 个 ， 而 RGB-D 原 理 较 复杂 ， 除 了 能 够 采集 到 彩色 图 片 之 外 ， 还 能 读 
出 每 个 像素 与 相机 之 间 的 距离 。 深 度 相 机 通 单 携带 多 个 摄像 头 ， 工 作 原 
理 和 普通 相机 不 尽 相 同 ， 在 第 5 讲 会 详细 介绍 其 工作 原理 ， 此 处 读者 只 需 
有 一 个 直观 概念 即 可 。 此 外 ，SLAM 中 还 有 全 景 相机 中 、Event 相 机 3 等 
特殊 或 新 兴 的 种 类 。 虽 然 偶 尔 能 看 到 它们 在 SLAM 中 的 应 用 ， 不 过 到 目前 
为 止 还 没有 成 为 主流 。 从 样子 上 看 ， 小 萝卜 使 用 的 似乎 是 双 目 相机 。 


一 n 


深度 相机 
=~ 


双 目 相机 
图 2-3 ”形形色色 的 相机 : 单 目 、 双 目 和 深度 相机 。 


我 们 来 分 别 看 一 看 各 种 相机 用 来 做 SLAM 时 有 什么 特点 。 
单 目 相机 


只 使 用 一 个 摄像 头 进行 SLAM 的 做 法 称 为 单 目 SLAM (Monocular 
SLAM) 。 这 种 传感器 结构 特别 简单 ， 成 本 特别 低 ， 所 以 单 目 SLAM 非 常 
受 研 究 者 关注 。 你 肯定 见 过 单 目 相机 的 数据 : 照片 。 是 的 ， 作 为 一 张 照 
片 ， 它 有 什么 特点 呢 ? 


照片 本 质 上 是 拍照 时 的 场景 (Scene) 在 相机 的 成 像 平面 上 留 下 的 一 
个 投影 。 它 以 二 维 的 形式 反映 了 三 维 的 世界 。 显然 ， 这 个 过 程 丢掉 了 场 
景 的 一 个 维度 ， 也 就 是 所 谓 的 深度 (REA) 。 在 单 目 相机 中 ， 我 们 无 
法 通过 单 张 图 片 来 计算 场景 中 物体 与 我 们 之 间 的 距离 (远近 ) 。 之 后 我 
们 会 看 到 ， 这 个 距离 将 是 SLAM 中 非常 关键 的 信息 。 由 于 我 们 人 类 见 过 大 
量 的 图 像 ， 形 成 了 一 种 天 生 的 直觉 ， 对 大 部 分 场景 都 有 一 个 直观 的 距离 
R 《空间 感 ) ， 它 可 以 帮助 我 们 判断 图 像 中 物体 的 远近 关系 。 比 如 说 ， 
我 们 能 够 辨认 出 图 像 中 的 物体 ， 并 且 知 道 其 大 致 的 大 小 ; 比如 ， 近 处 的 
物体 会 挡住 远 处 的 物体 ， 而 太阳 、 月 亮 等 天 体 一 般 在 很 远 的 地 方 ， 再 
如 ， 物 体 受 光照 后 会 留 下 影子 ， 等 等 。 这 些 信息 都 可 以 帮助 我 们 判断 物 
体 的 远近 ， 但 也 存在 一 些 情况 会 使 这 种 距离 感 失效 ， 这 时 我 们 就 无 法 判 
断 物 体 的 远近 及 其 真实 大 小 了 。 图 2-4 所 示 就 是 这 样 一 个 例子 。 在 这 张 图 
像 中 ， 我 们 无 法 仅 通 过 它 来 判断 后 面 那些 小 人 是 真实 的 人 ， 还 是 小 型 模 
型 。 除 非 我 们 转换 视角 ， 观 察 场景 的 三 维 结构 。 换 言 之 ， 在 单 张 图 像 
里 ， 你 无 法 确定 一 个 物体 的 真实 大 小 。 它 可 能 是 一 个 很 大 但 很 远 的 物 


单 目 相机 


体 ， 也 可 能 是 一 个 很 近 但 很 小 的 物体 。 由 于 近 大 远 小 的 原因 ， 它 们 可 能 
在 图 像 中 变 成 同样 大 小 的 样子 。 


f 
22-4 FAME AS: 不 知道 深度 时 ， 手 掌上 的 人 是 真人 还 是 模型 ? 


由 于 单 目 相机 拍摄 的 图 像 只 是 三 维 空间 的 二 维 投影 ， 所 以 ， 如 果真 
想 恢 复 三 维 结构 ， 必 须 改 变相 机 的 视角 。 在 单 目 SLAM 中 也 是 同样 的 原 
理 。 我 们 必须 移动 相机 ， 才 能 估计 它 的 运动 (Motion) ， 同 时 估计 场景 
中 物体 的 远 返 和 大 小 ， 不 妨 称 之 为 结构 (Structure) 。 那 么 ， 怎 么 估计 
这 些 运动 和 结构 呢 ? 从 生活 经 验 中 我 们 知道 ， 如 果 相 机 往 右 移动 ， 那 么 
图 像 里 的 东西 就 会 往 左 边 移 动 一 一 这 就 给 我 们 推测 运动 带 来 了 信息 。 另 
一 方面 ， 我 们 还 知道 : 近 处 的 物体 移动 快 ， 远 处 的 物体 则 运动 缓慢 。 于 
是 ， 当 相机 移动 时 ， 这 些 物体 在 图 像 上 的 运动 就 形成 了 视差 。 通 过 视 
差 ， 我 们 就 能 定量 地 判断 哪些 物体 离 得 远 ， 哪 些 物 体 离 得 近 。 

然而 ， 即 使 我 们 知道 了 物体 远近 ， 它 们 仍然 只 是 一 个 相对 的 值 。 比 
如 我 们 在 看 电影 时 ， 虽 然 能 够 知道 电影 场景 中 哪些 物体 比 另 一 些 大 ， 但 
无 法 确定 电影 里 那些 物体 的 “真实 尺度 ”: 那些 大 楼 是 真实 的 高 楼 大 厦 ， 
还 是 放 在 桌 上 的 模型 ”而 挫 毁 大 厦 的 是 真实 怪 团 ， 还 是 穿着 特 摄 服装 的 
演员 ? 直观 地 说 ， 如 果 把 相机 的 运动 和 场景 大 小 同时 放大 两 倍 ， 单 目 相 
机 所 看 到 的 像 是 一 样 的 。 同 样 地 ， 把 这 个 大 小 乘 以 任意 倍数 ， 我 们 都 将 
看 到 一 样 的 景象 。 这 说 明 ， 单 目 SLAM 估 计 的 轨迹 和 地 图 将 与 真实 的 轨迹 
和 地 图 相差 一 个 因子 ， 也 就 是 所 谓 的 尺度 (Scale) 中 。 由 于 单 目 SLAM 
无 法 仅 赁 图 像 确 定 这 个 真实 尺度 ， 所 以 又 称 为 尺度 不 确定 性 。 


平移 之 后 才能 计算 深度 ， 以 及 无 法 确定 真实 尺度 ， 这 两 件 事情 给 单 
目 SLAM 的 应 用 造成 了 很 大 的 麻烦 。 其 根本 原因 是 通过 单 张 图 像 无 法 确定 
深度 。 所 以 ， 为 了 得 到 这 个 深度 ， 人 们 开始 使 用 双 目 和 深度 相机 。 

双 目 相机 和 深度 相机 


使 用 双 目 相机 和 深度 相机 的 目的 ， 在 于 通过 某 种 手段 测量 物体 与 我 
们 之 间 的 距离 ， 克 服 单 目 相机 无 法 知道 距离 的 缺点 。 一 旦 知道 了 距离 ， 
场景 的 三 维 结构 就 可 以 通过 单个 图 像 恢复 出 来 ， 也 就 消除 了 尺度 不 确定 
性 。 尽 管 都 是 为 了 测量 距离 ， 但 双 目 相机 与 深度 相机 测量 深度 的 原理 是 
不 一 样 的 。 双 目 相 机 由 两 个 单 目 相机 组 成 ， 但 这 两 个 相机 之 间 的 距离 
( 称 为 基线 (Baseline) 】 是 已 知 的 。 我 们 通过 这 个 基线 来 估计 每 个 像素 
的 空间 位 置 一 一 这 和 人 有 眼 非常 相似 。 我 们 人 类 可 以 通过 左右 眼 图 像 的 差 
异 判 断 物体 的 远近 ， 在 计算 机 上 也 是 同样 的 道理 ( 见 图 2-5) 。 如 果 对 双 
目 相 机 进行 拓展 ， 也 可 以 描 建 多 目 相 机 ， 不 过 本 质 上 并 疫 有 什么 不 同 。 


图 2-5 ” 双 目 相机 的 数据 : 左 眼 图 像 ， 右 眼 图 像 。 通 过 左右 眼 的 差异 ， 能 够 判断 场景 中 物体 
与 相机 之 间 的 距离 。 


计算 机 上 的 双 目 相机 需要 大 量 的 计算 才能 (不 太 可 靠 地 ) 估计 每 一 
个 像素 点 的 深度 ， 相 比 于 人 类 真是 非常 笨拙 。 双 目 相 机 测量 到 的 深度 范 
围 与 基线 相关 。 基 线 距离 越 大 ， 能 够 测量 到 的 就 越 远 ， 所 以 无 人 车 上 拱 
载 的 双 目 通常 会 是 个 很 大 的 家 伙 。 双 目 相机 的 距离 估计 是 比较 左右 眼 的 
图 像 获得 的 ， 并 不 依赖 其 他 传 感 设 备 ， 所 以 它 既 可 以 应 用 在 室内 ， 亦 可 
应 用 于 室外 。 双 目 或 多 目 相机 的 缺点 是 配置 与 标定 均 较为 复杂 ， 其 深度 
量程 和 精度 受 双 目的 基线 与 分 辩 率 所 限 ， 而 且 视 差 的 计算 非常 消耗 计算 
资源 ， 需 要 使 用 GPU 和 FPGA 设 备 加 速 后 ， 才 能 实时 输出 整 张 图 像 的 距离 
言 息 。 因 此 在 现 有 的 条 件 下 ， 计 算 量 是 双 目 的 主要 问题 之 一 。 

深度 相机 (又 称 RGB-D 相 机 ， 在 本 书 中 主要 使 用 RGB-D 这 个 名 称 ) 
是 2010 年 左右 开始 兴起 的 一 种 相机 ， 它 最 大 的 特点 是 可 以 通过 红外 结构 
光 或 Time-of-Flight (ToF) 原理 ， 像 激光 传感器 那样 ， 通 过 主动 向 物体 发 


射 光 并 接收 返回 的 光 ， 测 出 物体 与 相机 之 间 的 距离 。 这 部 分 并 不 像 双 目 
相机 那样 通过 软件 计算 来 解决 ， 而 是 通过 物理 的 测量 手段 ， 所 以 相 比 于 
双 目 相机 可 节省 大 量 的 计算 ( 见 图 2-6) 。 目 前 常用 的 RGB-D 相 机 包括 
Kinect/Kinect V2, Xtion Pro Live、RealSense 等 。 不 过 ， 现 在 多 数 RGB-D 
相机 还 存在 测量 范围 窗 、 噪 声 大 、 视 野 小 、 易 受 日 光 干 扰 、 无 法 测量 透 
射 材质 等 诸多 问题 ， 在 SLAM 方 面 ， 主 要 用 于 室内 ， 室 外 则 较 难 应 用 。 


图 2-6 RGB-D 数 据 : 深度 相机 可 以 直接 测量 物体 的 图 像 和 距离 ， 从 而 恢复 三 维 结构 。 


我 们 讨论 了 几 种 单 见 的 相机 ， 相 信 通 过 以 上 的 说 明 ， 你 已 经 对 它们 
有 了 直观 的 了 解 。 现 在 ， 想 象 相机 在 场景 中 运动 的 过 程 ， 我 们 将 得 到 一 
系列 连续 变化 的 图 像 上 。 视 觉 SLAM 的 目标 ， 是 通过 这 样 的 一 些 图 像 ， 进 
行 定位 和 地 图 构建 。 这 件 事情 并 没有 我 们 想象 的 那么 简单 。 它 不 是 某 种 
算法 ， 只 要 我 们 输入 数据 ， 就 可 以 往外 不 断 地 输出 定位 和 地 图 信息 了 。 
SLAM 需 要 一 个 完善 的 算法 框架 ， 而 经 过 研究 者 们 长 期 的 努力 工作 ， 现 有 
x MER DARED To 


2.2 ”经 典 视觉 SLAM 框 架 


下 面 来 看 经 典 的 视觉 SLAM 框 架 ， 如 图 2-7 所 示 ， 了 解 一 下 视觉 
SLAM 究 竟 由 哪 几 个 模块 组 成 。 


aie ie 
ieee nae 2 
RRRS PA 视觉 里 程 计 非 线性 优化 
回环 检测 mi 


图 2-7 整体 视觉 SLAM 流 程 图 。 


整个 视觉 SLAM 流 程 包括 以 下 步骤 。 


1. 传 感 器 信息 读 取 。 在 视觉 SLAM 中 主要 为 相机 图 像 信息 的 读 取 和 预 
处 理 。 如 果 是 在 机 器 人 中 ， 还 可 能 有 码 盘 、 惯 性 传感器 等 信息 的 读 取 和 
同步 。 


2. 视 觉 里 程 计 (Visual Odometry, VO) o 视觉 里 程 计 的 任务 是 估算 
相 邻 图 像 间 相机 的 运动 ， 以 及 局 部 地 图 的 样子 。VO 又 称 为 前 端 (Front 
End) 。 


3. 后 端 优化 (Optimization) 。 后 端 接受 不 同时 刻 视觉 里 程 计 测量 的 
相机 位 姿 ， 以 及 回环 检测 的 信息 ， 对 它们 进行 优化 ， 得 到 全 局 一 致 的 轨 
迹 和 地 图 。 由 于 接 在 VO 之 后 ， 又 称 为 后 端 (Back End) o 


4. 回 环 检测 (Loop Closing) 。 回 环 检 测 判 断 机 器 人 是 否 到 达 过 先前 
的 位 置 。 如 果 检 测 到 回环 ， 它 会 把 信息 提供 给 后 端 进行 处 理 。 

5. 建 图 (Mapping) 。 它 根据 估计 的 轨迹 ， 建 立 与 任务 要 求 对 应 的 地 
Slo 


经 典 的 视觉 SLAM 框 架 是 过 去 十 几 年 的 研究 成 果 。 这 个 框架 本 身 及 其 
所 包含 的 算法 已 经 基本 定型 ， 并 且 已 经 在 许多 视觉 程序 库 和 机 器 人 程序 
库 中 提供 。 依 靠 这 些 算法 ， 我 们 能 够 构建 一 个 视觉 SLAM 系 统 ， 使 之 在 正 
常 的 工作 环境 里 实时 定位 与 建 图 。 因 此 ， 我 们 说 ， 如 果 把 工作 环境 限定 
在 静态 、 刚 体 ， 光 照 变化 不 明显 、 没 有 人 为 干扰 的 场景 BA, Xt 
SLAM 系 统 是 相当 成 熟 的 了 外 。 


读者 可 能 还 没有 理解 上 面 几 个 模块 的 概念 ， 下 面 就 来 详细 介绍 各 个 
模块 具体 的 任务 。 但 是 ， 准 确 理解 其 工作 原理 需要 一 些 数 学 知识 ， 我 们 
将 放 到 本 书 的 第 二 部 分 进行 。 这 里 读者 只 需 对 各 模块 有 一 个 直观 的 、 定 
性 的 理解 即 可 。 
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2.2.1 ”视觉 里 程 计 


视觉 里 程 计 关心 的 是 相 邻 图 像 之 间 的 相机 运动 ， 最 简单 的 情况 当然 
是 两 张 图 像 之 间 的 运动 关系 。 例 如 ， 当 看 到 图 2-8 时 ， 我 们 会 自然 地 反应 
出 右 图 应 该 是 左 图 向 左旋 转 一 定 角度 的 结果 (在 视频 情况 下 感觉 会 更 加 
AA) 。 我 们 不 妨 思考 一 下 : 自己 是 怎么 知道 “向 左旋 转 ” 这 件 事 情 的 
Me? 人 类 早已 习惯 于 用 眼睛 探索 世界 ， 估 计 自 己 的 位 置 ， 但 又 往往 难以 
用 理性 的 语言 描述 我 们 的 直觉 。 看 到 图 2-8 时 ， 我 们 会 自然 地 认为 ， 这 个 
场景 中 离 我 们 近 的 是 吧台 ， 远 处 是 墙壁 和 黑板 。 当 相机 向 左 转动 时 ， 吧 
台 离 我 们 近 的 部 分 出 现在 视野 中 ， 而 右 侧 远 处 的 柜子 则 移出 了 视野 。 通 
过 这 些 信息 ， 我 们 判断 相机 应 该 是 向 左旋 转 了 。 


S 
人 眼 反 应 的 运动 方向 


图 2-8 ”相机 拍摄 到 的 图 片 与 人 眼 反 应 的 运动 方向 。 


但 是 ， 如 果 进 一 步 问 : 能 否 确 定 旋转 了 多 少 度 ， 平 移 了 多 少 厘米 ? 
我 们 就 很 难 给 出 一 个 确切 的 答案 了 。 因 为 我 们 的 直觉 对 这 些 具体 的 数字 
并 不 敏感 。 但 是 ， 在 计算 机 中 ， 又 必须 精确 地 测量 这 段 运动 信息 。 所 以 
我 们 要 问 : 计算 机 是 如 何 通过 图 像 确定 相机 的 运动 的 呢 ? 

前 面 也 提 过 ， 在 计算 机 视觉 领域 ， 人 类 在 直觉 上 看 来 十 分 自然 的 事 
情 ， 在 计算 机 视觉 中 却 非常 困难 。 图 像 在 计算 机 里 只 是 一 个 数值 矩阵 。 
这 个 矩阵 里 表达 着 什么 东西 ， 计 算 机 富 无 概念 (这 也 正 是 现在 机 器 学 习 
要 解决 的 问题 ) 。 而 在 视觉 SLAM 中 ， 我 们 只 能 看 到 一 个 个 像素 ， 知 道 它 
们 是 某 些 空间 点 在 相机 的 成 像 平 面 上 投影 的 结果 。 所 以 ， 为 了 定量 地 估 
计 相 机 运动 ， 必 须 先 了 解 相机 与 空间 点 的 几何 关系 。 


要 讲 清 这 个 几何 关系 以 及 VO 的 实现 方法 ， 需 要 铺垫 一 些 背 景 知 识 。 
在 这 里 我 们 先 让 读者 对 VO 有 个 直观 的 概念 。 现 在 只 需 知道 ，VO 能 够 通 
过 相 邻 帧 间 的 图 像 估计 相机 运动 ， 并 恢复 场景 的 空间 结构 。 称 它 为 “里 程 
计 ” 是 因为 它 和 实际 的 里 程 计 一 样 ， 只 计算 相 邻 时 刻 的 运动 ， 而 和 再 往 前 


的 过 去 的 信息 没有 关联 。 在 这 一 点 上 ，VO 就 像 一 种 只 有 短 时 间 记 忆 的 物 
种 。 


现在 ， 假 定 我 们 已 有 了 一 个 视觉 里 程 计 ， 估计 了 两 张 图 像 间 的 相机 
运动 。 那 么 ， 只 要 把 相 邻 时 刻 的 运动 * 串 ”起 来 ， 就 构成 了 机 器 人 的 运动 
轨迹 ， 从 而 解决 了 定位 问题 。 另 一 方面 ， 我 们 根据 每 个 时 刻 的 相机 位 
置 ， 计 算出 各 像素 对 应 的 空间 点 的 位 置 ， 就 得 到 了 地 图 。 这 么 说 来 ， 有 
了 VO， 是 不 是 就 解决 了 SLAM 问 题 呢 ? 


视觉 里 程 计 确 实 是 SLAM 的 关键 ， 我 们 也 会 伦 大 量 的 篇 幅 来 介绍 它 。 
然而 ， 仅 通过 视觉 里 程 计 来 估计 轨迹 ， 将 不 可 避免 地 出 现 累 积 漂移 
(Accumulating Drift) 。 这 是 由 于 视觉 里 程 计 (在 最 简单 的 情况 下 ) 只 估 
计 两 个 图 像 间 的 运动 造成 的 。 我 们 知道 ， 每 次 估计 都 带 有 一 定 的 误差 ， 
而 由 于 里 程 计 的 工作 方式 ， 先 前 时 刻 的 误差 将 会 传递 到 下 一 时 刻 ， 导 致 
经 过 一 段 时 间 之 后 ， 估 计 的 轨迹 将 不 再 准确 〈 见 图 2-9) 。 比 方 说 ， 机 器 
人 先 向 左 转 90" ， 再 向 右 转 90 " 。 由 于 误差 ， 我 们 把 第 一 个 90 估计 成 了 
89 ` 。 那 我 们 就 会 尴 傣 地 发 现 ， 向 右 转 之 后 机 器 人 的 估计 位 置 并 没有 回 到 
原点 。 更 糟糕 的 是 ， 即 使 之 后 的 估计 再 准确 ， 与 真实 值 相 比 ， 都 会 带 上 
这 -1 的 误差 。 
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累积 误差 导致 
长 时 间 估计 不 再 准确 
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需要 回环 检测 和 全 局 校正 心 - 


图 2-9 ”累积 误差 与 回环 检测 的 校正 结果 "9 o 


这 也 就 是 所 谓 的 漂移 (Drift) 。 它 将 导致 我 们 无 法 建立 一 致 的 地 
图 。 你 会 发 现 原本 直 的 走廊 变 成 了 斜 的 ， 而 原本 90 WAAR SAS BY 
这 实在 是 一 件 很 难 令 人 忍受 的 事情 ! 为 了 解决 漂移 问题 ， 我 们 还 需 
要 两 种 技术 : 后 端 优 化 外 和 回环 检测 。 回 环 检测 负责 把 * 机 器 人 回 到 原始 


位 置 ? 的 事情 检测 出 来 ， 而 后 端 优化 则 根据 该 信息 ， 校 正 整个 轨迹 的 形 


o 


2.2.2 ”后 端 优化 


笼统 地 说 ， 后 端 优化 主要 指 处理 SLAM 过 程 中 噪声 的 问题 。 虽 然 我 
们 很 希望 所 有 的 数据 都 是 准确 的 ， 然 而 现实 中 ， 再 精确 的 传感器 也 带 有 
一 定 的 噪声 。 便 宜 的 传感器 测量 误差 较 大 ， 昂 贵 的 可 能 会 小 一 些 ， 有 的 
传感器 还 会 受 磁 场 、 温 度 的 影响 。 所 以 ， 除 了 解决 “如 何 从 图 像 估 计 出 相 
机 运动 ”之 外 ， 我 们 还 要 关心 这 个 估计 带 有 多 大 的 噪声 ， 这 些 噪 声 是 如 何 
从 上 一 时 刻 传递 到 下 一 时 刻 的 ， 而 我 们 又 对 当前 的 估计 有 多 大 的 自信 。 
后 端 优化 要 考虑 的 问题 ， 就 是 如 何 从 这 些 带 有 噪声 的 数据 中 估计 整个 系 
统 的 状态 ， 以 及 这 个 状态 估计 的 不 确定 性 有 多 大 一 一 这 称 为 最 大 后 验 概 
率 估计 (Maximum-a-Posteriori, MAP) 。 这 里 的 状态 既 包 括 机 器 人 自身 
的 轨迹 ， 也 包含 地 图 。 


相对 地 ， 视 觉 里 程 计 部 分 有 时 被 称 为 “前 端 "。 在 SLAM 框 染 中 ， 前 端 
给 后 端 提供 待 优 化 的 数据 ， 以 及 这 些 数据 的 初始 值 。 而 后 端 负责 整体 的 
优化 过 程 ， 它 往往 面 对 的 只 有 数据 ， 不 必 关 心 这 些 数 据 到 底 来 自 什 么 传 
感 器 。 在 视觉 S LAM 中 ， 前 端 和 计算 机 视觉 研究 领域 更 为 相关 ， 比 如 图 
像 的 特征 提取 与 匹配 等 ， 后 端 则 主要 是 滤波 与 非 线性 优化 算法 。 


从 历史 意义 上 来 说 ， 现 在 我 们 称 为 后 端 优化 的 部 分 ， 很 长 一 段 时 间 
直接 被 称 为 “<SLAM 研 究 "。 早 期 的 SLAM 问 题 是 一 个 状态 估计 间 题 一 一 正 
是 后 端 优化 要 解决 的 东西 。 在 最 早 提出 SLAM 的 一 系列 论文 中 ， 当 时 的 人 
们 称 它 为 “空间 状态 不 确定 性 的 估计 ” (Spatial Uncertainty) H, RAS 
一 些 隆 涩 ， 但 也 确实 反映 出 了 SLAM 问 题 的 本 质 : 对 运动 主体 自身 和 周围 
环境 空间 不 确定 性 的 估计 。 为 了 解决 SLAM 问 题 ， 我 们 需要 状态 估计 理 
论 ， 把 定位 和 建 图 的 不 确定 性 表达 出 来 ， 然 后 采用 滤波 器 或 非 线 性 优 
化 ， 估 计 状 态 的 均值 和 不 确定 性 (方差 ) 。 状 态 估 计 与 非 线 性 优化 的 具 
体内 容 将 在 第 6 讲 、 第 10 讲 和 第 11 讲 介绍 。 让 我 们 暂时 跳 过 它 的 原理 说 
明 ， 继 续 往 下 介绍 。 


2.2.3 ”回环 检测 


回环 检测 ， 又 称 闭环 检测 (Loop Closure Detection) ， 主 要 解决 位 置 
估计 随时 间 漂 移 的 问题 。 怎 么 解决 呢 ? 假设 实际 情况 下 机 器 人 经 过 一 段 
时 间 的 运动 后 回 到 了 原点 ， 但 是 由 于 漂移 ， 它 的 位 置 估计 值 却 没 有 回 到 
原点 。 怎 么 办 呢 ? 我们 想 ， 如 果 有 某 种 手段 ， 让 机 器 人 知道 * 回 到 了 原 
点 ”这 件 事 ， 或 者 把 “原点 ”识别 出 来 ， 我 们 再 把 位 置 估计 值 “ 拉 ”过 去 ， 就 
可 以 消除 漂移 了 。 这 就 是 所 谓 的 回环 检测 。 


回环 检测 与 “定位 "和 “ 建 图 "二 者 都 有 密切 的 关系 。 事 实 上 ， 我 们 认 
为 ， 地 图 存在 的 主要 意义 是 让 机 器 人 知晓 自己 到 过 的 地 方 。 为 了 实现 回 
环 检测 ， 我 们 需要 让 机 器 人 具有 识别 到 过 的 场景 的 能 力 。 它 的 实现 手段 
有 很 多 。 例 如 像 前 面 说 的 那样 ， 我 们 可 以 在 机 器 人 下 方 设置 一 个 标志 物 
(如 一 张 二 维 码 图 片 ) 。 它 只 要 看 到 了 这 个 标志 ， 就 知道 自己 回 到 了 原 
点 。 但 是 ， 该 标志 物 实质 上 是 一 种 环境 中 的 传感器 ， 对 应 用 环境 做 了 限 
制 (万 一 不 能 贴 二 维 码 怎么 办 ? ) 。 我 们 更 希望 机 器 人 能 使 用 携带 的 传 
感 器 一 也 就 是 图 像 本 身 ， 来 完成 这 一 任务 。 例 如 ， 可 以 判断 图 像 间 的 
相似 性 来 完成 回环 检测 。 这 一 点 和 人 是 相似 的 。 当 我 们 看 到 两 张 相似 的 
图 片 时 ， 容 易 辨认 它们 来 自 同一 个 地 方 。 如 果 回环 检测 成 功 ， 可 以 显著 
地 减 小 累积 误差 。 所 以 ， 视 觉 回环 检测 实质 上 是 一 种 计算 图 像 数据 相似 
性 的 算法 。 由 于 图 像 的 信息 非常 丰富 ， 使 得 正确 检测 回环 的 难度 降低 了 
不 少 。 

在 检测 到 回环 之 后 ， 我 们 会 把 “A 与 B 是 同一 个 点 ”这样 的 信息 告诉 后 
端 优化 算法 。 然 后 ， 后 端 根据 这 些 新 的 信息 ， 把 轨迹 和 地 图 调整 到 符合 
回环 检测 结果 的 样子 。 这 样 ， 如 果 我 们 有 充分 而 且 正 确 的 回环 检测 ， 就 
可 以 消除 累积 误差 ， 得 到 全 局 一 致 的 轨迹 和 地 图 。 


2.24 BE 


建 图 (Mapping) 是 指 构 建 地 图 的 过 程 。 地 图 ( 见 图 2-10) 是 对 环境 
的 描述 ， 但 这 个 描述 并 不 是 固定 的 ， 需 要 视 SLAM 的 应 用 而 定 。 


2D 栅 格 地 图 2D 拓扑 地 图 


3D 点 云 地 图 3D 网 格 地 图 


图 2-10 “形形色色 的 地 图 "2 。 


对 于 家 用 扫地 机 器 人 来 说 ， 这 种 主要 在 低 矮 平面 里 运动 的 机 器 人 ， 
只 需要 一 个 二 维 的 地 图 ， 标 记 哪 里 可 以 通过 ， 哪 里 存在 障碍 物 ， 就 够 它 
在 一 定 范围 内 导航 了 。 市 对 于 一 个 相机 ， 它 有 6 自由 度 的 运动 ， 我 们 至 少 
需要 一 张 三 维 的 地 图 。 有 些 时候 ， 我 们 想 要 一 个 漂亮 的 重建 结果 ， 不 仅 

是 一 组 空间 点 ， 还 需要 融 纹 理 的 三 角 面 片 。 另 一 些 时 候 ， 我 们 又 不 关心 
地 图 的 样子 ， 只 需要 知道 “A 扣 到 B 扣 可 通过 ， 而 B 扣 到 C 操 不行 "这样 的 事 
情 。 甚 至 ， 有 时 不 需要 地 图 ， 或 者 地 图 可 以 由 其 他 人 提供 ， 例 如， 行驶 
的 车 辆 往往 可 以 得 到 已 绘制 好 的 当地 地 图 。 


对 于 地 图 ， 我 们 有 太 多 的 想法 和 需求 。 因 此 ， 相 比 于 前 面 提 到 的 视 
觉 里 程 计 、 回 环 检测 和 后 端 优化 ， 建 图 并 没有 一 个 固定 的 形式 和 算法 。 
一 组 空间 点 的 集合 也 可 以 称 为 地 图 ， 一 个 漂亮 的 3D 模 型 亦 是 地 图 ， 一 个 
标记 着 城市 、 村 庄 、 铁 路 、 河 道 的 图 片 还 是 地 图 。 地 图 的 形式 随 SLAM 的 
应 用 场合 而 定 。 大 体 上 讲 ， 可 以 分 为 度量 地 图 与 拓扑 地 图 两 种 。 


度量 地 图 (Metric Map) 


度量 地 图 强调 精确 地 表示 地 图 中 物体 的 位 置 关 系 ， 通 常用 稀 玻 
(Sparse) 与 稠密 (Dense) 对 其 分 类 。 稀 跑 地 图 进行 了 一 定 程度 的 抽 


象 ， 并 不 需要 表达 所 有 的 物体 。 人 例如， 我们 选择 一 部 分 具有 代表 意义 的 
东西 ， 称 之 为 路 标 (Landmark) ， 那 么 一 张 稀 疏 地 图 就 是 由 路 标 组 成 的 
地 图 ， 而 不 是 路 标的 部 分 就 可 以 忽略 掉 。 相 对 地 ， 笛 密 地 图 着 重 于 建 模 
所 有 看 到 的 东西 。 对 于 定位 来 说 ， 稀 下 路 标 地 图 就 足够 了 。 而 用 于 导航 
时 ， 则 往往 需要 稠密 的 地 图 (否则 撞 上 两 个 路 标 之 间 的 墙 怎么 办 ? ) 。 
稠密 地 图 通常 按照 某 种 分 辨 率 ， 由 许多 个 小 块 组 成 。 对 于 二 维度 量 地 图 
是 许多 个 小 格子 (Grid) ， 而 对 于 三 维度 量 地 图 则 是 许多 小 方块 
(Voxel) 。 一 般 地 ， 一 个 小 块 含 有 占据 、 空 六、 未 知 三 种 状态 ， 以 表达 
该 格 内 是 否 有 物体 。 当 查询 某 个 空间 位 置 时 ， 地 图 能 够 给 出 该 位 置 是 否 
可 以 通过 的 信息 。 这 样 的 地 图 可 以 用 于 各 种 导航 算法 ， 如 A*、D*!9 等 ， 
为 机 器 人 研究 者 所 重视 。 但 是 我 们 也 看 到 ， 这 种 地 图 需要 存储 每 一 个 格 
点 的 状态 ， 会 耗费 大 量 的 存储 空间 ， 而 且 多 数 情 况 下 地 图 的 许多 细节 音 
分 是 无 用 的 。 另 一 方面 ， 大 规模 度量 地 图 有 时 会 出 现 一 致 性 问题 。 很 小 
的 一 点 转向 误差 ， 可 能 会 导致 两 辣 屋 子 的 墙 出 现 重 亚 ， 使 地 图 失效 。 


拓扑 地 图 (Topological Map) 


相 比 于 度量 地 图 的 精确 性 ， 拓 扑 地 图 则 更 强调 地 图 元 素 之 间 的 关 
系 。 拓 扑 地 图 是 一 个 图 (Graph) ， 由 节点 和 边 组 成 ， 只 考虑 节点 间 的 连 
通 性 ， 例 如 A、B 点 是 连通 的 ， 而 不 考虑 如 何 从 A 点 到 达 B 点 。 它 放松 了 地 
图 对 精确 位 置 的 需要 ， 去 挥 了 地 图 的 细节 问题 ， 是 一 种 更 为 紧凑 的 表达 
方式 。 然 而 ， 拓 扑 地 图 不 擅长 表达 具有 复杂 结构 的 地 图 。 如 何 对 地 图 进 
行 分 割 形成 结 点 与 边 ， 又 如 何 使 用 拓扑 地 图 进行 导航 与 路 径 规 划 ， 仍 是 
有 待 研究 的 问题 。 


2.3 SLAM 问题 的 数学 表述 


通过 前 面 的 介绍 ， 读 者 应 该 对 SLAM 中 各 个 模块 的 组 成 和 主要 功能 
了 直观 的 了 解 。 但 仅仅 靠 直 观 印象 并 不 能 帮助 我 们 写 出 可 以 运行 的 程 
序 。 我 们 要 把 它 上 升 到 理性 层次 ， 也 就 是 用 数学 语言 来 描述 SLAM 过 程 。 
我 们 会 用 到 一 些 变 量 和 公式 ,但 请 读者 放心 ， 我 们 会 尽量 让 它 保 持 足够 
地 清楚 。 

假设 小 萝卜 正 携带 着 某 种 传感器 在 未 知 环境 里 运动 ， 怎 么 用 数学 语 
言 描 述 这 件 事 呢 ? 首先 ， 由 于 相机 通常 是 在 某 些 时 刻 采 集 数据 的 ， 所 以 
我 们 也 只 关心 这 些 时 刻 的 位 置 和 地 图 。 这 就 把 一 段 连续 时 间 的 运动 变 成 


了 离散 时 刻 + =1,…,K 当中 发 生 的 事情 。 在 这 些 时 刻 ， 用 x 表示 小 萝卜 自身 
的 位 置 。 于 是 各 时 刻 的 位 置 就 记 为 x ,…xx ， 它 们 构成 了 小 萝卜 的 轨迹 。 
地 图 方面 ， 我 们 假设 地 图 是 由 许多 个 路 标 (Landmark) 组 成 的 ， 而 每 个 
时 刻 ， 传 感 器 会 测量 到 一 部 分 路 标点 ， 得 到 它们 的 观测 数据 。 不 妨 设 路 
标点 一 共有 N 个 ， 用 y yy, 表示 它们 。 在 这 样 的 设 定 中 ,“ 小 萝卜 携带 
着 传感器 在 环境 中 运动 "， 由 如 下 两 件 事情 描述 : 

1. 什 么 是 运动 ? 我 们 要 考虑 从 k 1 时 刻 到 k 时 刻 ， 小 萝卜 的 位 置 x 是 
如 何 变化 的 。 

2. 什 么 是 观测 ? 假设 小 萝卜 在 k 时 刻 于 x, 处 探测 到 了 某 一 个 路 标 ) ， 
我 们 要 考虑 这 件 事情 是 如 何 用 数学 语言 来 描述 的 。 

先 来 看 运动 。 通 常 ， 机 器 人 会 携带 一 个 测量 自身 运动 的 传感器 ， 比 
如 说 码 盘 或 惯性 传感器 。 这 个 传感器 可 以 测量 有 关 运动 的 读数 ， 但 不 一 
定 直接 就 是 位 置 之 差 ， 还 可 能 是 加 速度 、 角 速度 等 信息 。 然 而 ， 无 论 是 
什么 传感器 ， 我 们 都 能 使 用 一 个 通用 的 、 抽 象 的 数学 模型 


Lk = f (Lk—1, Uk, Wk) - (2.1) 


这 里 ， 必 是 运动 传感器 的 读数 (有 时 也 叫 输入 ) ，w, 为 噪声 。 注 意 
到 ， 我 们 用 一 个 一 般 函 数 来 描述 这 个 过 程 ， 而 不 具体 指明 /的 作用 方 
式 。 这 使 得 整个 函数 可 以 指 代 任意 的 运动 传感器 ， 成 为 一 个 通用 的 方 
程 ， 而 不 必 限 定 于 某 个 特殊 的 传感器 上 。 我 们 把 它 称 为 运动 方程 。 

与 运动 方程 相对 应 ， 还 有 一 个 观测 方程 。 观 测 方程 描述 的 是 ， 当 小 
萝卜 在 x, 位 置 上 看 到 某 个 路 标点 y ， 产 生 了 一 个 观测 数据 z, 。 同 样 ， 用 
一 个 抽象 的 函数 h 来 描述 这 个 关系 : 


Zk j = h (Yj, Tk, vkj) - (2.2) 


XE, vy 是 这 次 观测 里 的 噪声 。 由 于 观测 所 用 的 传感器 形式 更 多 ， 
这 里 的 观测 数据 z 以 及 观测 方程 h 也 有 许多 不 同 的 形式 。 
读者 或 许 会 说 ， 我 们 用 的 水 数 fh ， 似 乎 并 没有 具体 地 说 明 运 动 和 观 
测 是 怎么 回 事 ? 同时 ， 这 里 的 x ,y ,z 又 是 什么 东西 呢 ? 事实 上 ， 根 据 小 萝 
h 的 真实 运动 和 传感器 的 种 类 ， 存 在 着 若干 种 参数 化 方式 
(Parameterization) 。 什 么 叫 参 数 化 呢 ? 举例 来 说 ， 假 设 小 萝卜 在 平面 中 


运动 ， 那 么 ， 它 的 位 姿 中 由 两 个 位 置 和 一 个 转角 来 描述 ， 即 x, =v Af. 
同时 ， 运 动 传感器 能 够 测量 到 小 萝卜 在 任意 两 个 时 间 间 隔 位 置 和 转角 的 
变化 量 w = [Az,Ay, 人 起， 于 是 ， 此 时 运动 方程 就 可 以 具体 化 为 这 是 简单 的 
线性 关系 。 不 过 ， 并 不 是 所 有 的 传感器 都 能 直接 测量 出 位 移 和 角度 变 
化 ， 所 以 也 存在 着 其 他 形式 更 加 复杂 的 运动 方程 ， 那 时 我 们 可 能 需要 进 
行动 力学 分 析 。 关 于 观测 方程 ， 比 方 说 小 萝卜 携 市 着 一 个 二 维 激光 传 感 
器 。 我 们 知道 激光 传感器 观测 一 个 2D 路 标点 时 ， 能 够 测 到 两 个 量 : 路 标 
点 与 小 萝卜 本 体 之 间 的 距离 r 和 夹 角 @ 。 记 路 标点 为 y =[p, ,p, J (为 保持 


faa, BESS Kin) ， 观 测 数 据 为 z =[n,9 ] ， 那 么 观测 方程 就 具体 化 
为 : 


x x Ax 
Y = Y + Ay + Wk. (2.3) 
0 0 A0 

k k—1 k 


Vps — 2)? + (py - y)? 


M 
@ arctan (2 a=) 


考虑 视觉 SLAM 时 ,传感器 是 相机 ， 那 么 观测 方程 就 是 “对 路 标点 拍 
摄 后 ， 得 到 图 像 中 的 像素 ”的 过 程 。 这 个 过 程 军 涉 到 相机 模型 的 描述 ， 将 
在 第 5 讲 中 详细 介绍 ， 这 里 暂且 略 过 。 

可 见 ， 针 对 不 同 的 传感器 ， 这 两 个 方程 有 不 同 的 参数 化 形式 。 如 果 
我 们 保持 通用 性 ， 把 它们 取 成 通用 的 抽象 形式 ， 那 么 SLAM 过 程 可 总 结 为 
两 个 基本 方程 : 


+v. (2.4) 


| Tk = f (Lk-1, Uk, We) (2.5) 


Zk j = h (Yj, Ek, Ve,j) 


这 两 个 方程 描述 了 最 基本 的 SLAM 问 题 : 当知 道 运 动 测 量 的 读数 u ， 
以 及 传感器 的 读数 z 时， 如 何 求解 定位 问题 (估计 x ) 和 建 图 问题 (估计 y 


) ? 这 时 ， 我 们 就 把 SLAM 问 题 建 模 成 了 一 个 状态 估计 问题 : 如 何 通过 
带 有 噪声 的 测量 数据 ， 估 计 内 部 的 、 隐 藏 着 的 状态 变量 ? 


状态 估计 问题 的 求解 ， 与 两 个 方程 的 具体 形式 ， 以 及 噪声 服从 哪 种 
分 布 有 天。 按照 运动 和 观测 方程 是 否 为 线性 ， 噪 声 是 否 服从 高 斯 分 布 进 
行 分 类 ， 分 为 线性 / 非 线性 和 高 斯 / 非 高 斯 系统 。 其 中 线性 高 斯 系统 

(Linear Gaussian，LG 系 统 ) 是 最 简单 的 ， 它 的 无 偏 的 最 优 估计 可 以 由 卡 

尔 曼 滤波 器 (Kalman Filter, KF) 给 出 。 而 在 复杂 的 非 线 性 非 高 斯 系统 
(Non-Linear Non-Gaussian，NLNG 系 统 ) 中 ， 我 们 会 使 用 以 扩展 卡尔 曼 
滤波 器 (Extended Kalman Filter, EKF) 和 非 线 性 优化 两 大 类 方法 去 求 
解 。 直 至 21 世 纪 早 期 ， 以 EKF 为 主 的 滤波 器 方法 在 SLAM 中 占据 了 主导 地 
位 。 我 们 会 在 工作 点 处 把 系统 线性 化 ， 并 以 预测 一 更 新 两 大 步骤 进行 求 
fe ( 见 第 10 讲 ) 。 最 早 的 实时 视觉 SLAM 系 统 即 是 基于 EKFW 开发 的 。 随 
后 ， 为 了 克服 EKF 的 缺点 (例如 线性 化 误差 和 噪声 高 斯 分 布 假设 ) ， 人 
们 开始 使 用 粒子 滤波 器 (Particle Filter) 等 其 他 滤波 器 ， 乃 至 使 用 非 线 性 
优化 的 方法 。 时 至 今日 ， 主 流 视 觉 SLAM 使 用 以 图 优化 (Graph 
Optimization) 为 代表 的 优化 技术 进行 状态 估计 5 。 我 们 认为 优化 技术 已 
经 明显 优 于 滤波 器 技术 ， 只 要 计算 资源 允许 ， 通 常 都 偏向 于 使 用 优化 方 
法 〈 见 第 10 讲 和 第 11 讲 ) 。 


相信 读者 已 经 对 SLAM 的 数学 模型 有 了 大 致 的 了 解 ， 然 而 我 们 仍 需 澄 
清 一 些 问题 。 首 先 ， 要 说 明 机 器 人 位 置 x 是 什么 。 我 们 方才 说 位 置 是 有 
些 模糊 的 。 也 许 读者 能 够 理解 ， 在 平面 中 运动 的 小 萝卜 可 以 用 两 个 坐标 
加 一 个 转角 的 形式 将 位 置 参 数 化 。 然 而 ， 虽 然 我 的 漫画 风格 有 些 二 次 
元 ， 小 萝卜 在 更 多 时 候 是 一 个 三 维 空间 里 的 机 器 人 。 我 们 知道 三 维 空间 
的 运动 由 3 个 轴 构 成 ， 所 以 小 萝卜 的 运动 要 由 3 个 轴 上 的 平移 ， 以 及 绕 着 3 
个 轴 的 旋转 来 描述 ， 一 共有 6 个 自由 度 。 那 是 否 意味 着 随便 用 一 个 Rs 中 的 
向 量 就 能 描述 它 了 呢 ? 我 们 将 发 现 事情 并 没有 那么 简单 。 对 6 自由 度 的 位 
姿 是， 如 何 表 达 它 ， 如 何 优化 它 ， 都 需要 一 定 篇 幅 来 介绍 ， 这 将 是 第 3 讲 
和 第 4 讲 的 主要 内 容 。 随 后 ， 我 们 要 说 明 在 视觉 SLAM 中 ， 观 测 方程 如 何 
参数 化 。 换 句 话 说 ， 空 间 中 的 路 标点 是 如 何 投 影 到 一 张 照 片上 的 。 这 需 
要 解释 相机 的 成 像 模 型 ， 我 们 将 在 第 5 讲 介 绍 。 最 后 ， 当 知道 了 这 些 信 
息 ， 怎 么 求解 上 述 方程 ”这 需要 非 线性 优化 的 知识 ， 是 第 6 讲 的 内 容 。 


这 些 内 容 组 成 了 本 书 数学 知识 的 部 分 。 在 对 它们 进行 铺垫 之 后 ， 我 
们 融 能 仔细 讨论 视觉 里 程 计 、 后 端 优化 等 更 详细 的 知识 了 。 可 以 看 到 ， 


本 讲 介绍 的 内 容 构 成 了 本 书 的 一 个 提要 。 如 果 读 者 还 没有 很 好 地 理解 上 
面 的 概念 ， 不 妨 回 过 头 再 阅读 一 遍 。 下 面 就 要 开始 介绍 程序 啦 ! 


2.4 实践: 编程 基础 
2.4.1 ”安装 Linux 操 作 系 统 


终于 开始 令 人 兴奋 的 实践 环节 啦 ! 你 是 否 准 备 好 了 呢 ? 为 了 完成 本 
书 的 实践 环节 ， 我 们 需要 准备 一 台电 脑 。 你 可 以 使 用 笔记 本 或 台式 机 ， 
当然 最 好 是 你 个 人 的 电脑 ， 因 为 我 们 需要 在 上 面 安装 操作 系统 进行 实 


验 。 


我 们 的 程序 将 以 Linux 上 的 C++ 程序 为 主 。 在 实验 过 程 中 ， 我 们 会 使 
用 到 大 量程 序 库 。 大 部 分 程序 库 只 对 Linux 提 供 了 较 好 的 支持 ， 而 在 
Windows 上 的 配置 则 相对 (相当 ) 麻烦 。 因 此 ， 我 们 不 得 不 假定 你 已 经 具 
备 关 于 Linux 的 基本 知识 了 (参见 上 一 讲 的 练习 题 ， 包 括 使 用 基本 的 命 
令 ， 了 解 软件 如 何 安装 。 这 样 我 们 才 无 须 讲 解 这 些 内 容 。 当 然 ， 你 不 必 
了 解 如 何在 Linux 下 开发 C++ 程 序 ， 这 正 是 下 面 会 详细 讲解 的 。 


我 们 先 来 搭建 本 书 所 需 的 实验 环境 。 作 为 一 本 面向 初学 者 的 书 ， 我 
们 使 用 Ubuntu 作为 开发 环境 。 在 Linux 的 各 大 发 行 版 中 ，Ubuntu 及 其 衍生 
版 本 一 直 享 有 对 用 户 友 好 的 美誉 。Ubuntu 是 一 个 开源 操作 系统 ， 它 的 系 
统 和 软件 可 以 在 官方 网 站 (http:/cn.ubuntu.com) 免费 下 载 ， 并 且 提 供 了 
详细 的 安装 方式 说 明 。 同 时 ， 清 华 、 中 科大 等 国内 各 大 高 校 也 提供 了 
Ubuntu 软件 源 ， 使 软件 的 安装 十 分 便捷 。 对 于 初学 者 ， 建 议 你 使 用 和 我 
们 一 样 的 环境 : Ubuntu 14.04。 如 果 你 想 试 试 其 他 口味 ， 那 么 Ubuntu 
16.04, Ubuntu Kylin, Debian 7/8 和 Linux Mint 17/18 也 是 不 错 的 选择 。 我 
们 保证 书 中 所 有 代码 在 Ubuntu 14.04 下 经 过 了 良好 的 测试 ， 但 如 果 你 选择 
其 他 发 行 版 ， 则 无 法 确定 你 是 否 会 遇 到 问题 。 你 可 能 需要 花费 一 些 时 间 
解决 问题 (不 过 你 也 可 以 把 它们 当 作 锻炼 自己 的 机 会 ) 。 大 体 来 说 ， 
Ubuntu 对 各 种 库 的 支持 均 较为 完善 ， 软 件 也 非常 丰富 。 尽 管 我 们 不 限制 
你 具体 使 用 哪 种 Linux 发 行 版 ， 不 过 在 讲解 中 ， 我 们 会 以 Ubuntu 14.04 为 
例 ， 且 主要 使 用 Ubuntu 下 的 命令 (例如 apt-get) ， 就 不 谈 在 其 他 Linux 下 
怎么 操作 了 。 一 般 情 况 下 ， 程 序 在 Linux 间 移植 不 会 非常 烦琐 。 但 如 果 你 
想 在 Windows 或 OS X 下 使 用 本 书 中 的 程序 ， 则 需要 有 一 定 的 移植 经 验 。 


现在 ， 请 大 家 在 自己 的 PC 上 安装 好 Ubuntu 14.04。 关 于 Ubuntu 的 安 
装 ， 可 以 在 网 上 搜 到 大 量 教 程 ， 只 要 照 做 即 可 ， 此 处 略 过 。 最 简单 的 方 
式 是 使 用 虚拟 机 ( 见 图 2-11) ， 但 需要 占用 大 量 内 存 (我 们 的 经 验 是 4GB 
以 上 ) 和 CPU 才能 保持 流畅 ; 你 也 可 以 安装 双 系 统 ， 这 样 会 快 一 些 ， 但 
需要 一 个 空白 的 U 盘 来 作为 启动 盘 。 另 外 ， 虚 拟 机 软件 对 外 部 硬件 的 支持 
往往 不 够 好 ， 如 果 希 望 使 用 实际 的 传感器 〈 双 目 、Kinect 等 ) ， 则 建议 你 
使 用 双 系 统 来 安装 Linux。 


图 2-11 一 个 运行 在 虚拟 机 中 的 Ubuntu 14.04。 


关于 安装 的 小 提示 : 

“安装 操作 系统 时 请 不 要 选择 “安装 中 下 载 更 新 ”， 并 且 断 开 网 络 连 
接 ， 这 样 可 以 提高 安装 速度 。 至 于 更 新 可 以 在 系统 安装 完毕 后 再 装 。 如 
果 你 有 SSD 硬 盘 ， 这 个 过 程 大 概 用 时 15 分 钟 。 

“安装 完成 后 ， 请 务必 把 软件 源 设置 到 离 你 较 近 的 服务 器 上 ， 以 获得 
更 快 的 下 载 速度 。 例 如 我 们 使 用 清华 的 软件 源 通 常 能 以 10MB/s 的 速度 安 
装 软件 外。 

现在 ， 假 设 你 已 经 成 功 安装 好 Ubuntu， 无 论 是 使 用 虚拟 机 还 是 双 系 
统 的 方式 。 如 果 你 还 不 熟悉 Ubuntu， 可 以 试 试 它 的 各 种 软件 ， 体 验 一 下 
它 的 界面 和 交互 方式 00 。 不 过 我 必须 提醒 你， 特别 是 新 手 朋 友 : 不 要 在 
Ubuntu 的 用 户 界面 上 花费 太 多 时 间 ! Linux 有 许多 可 能 浪费 时 间 的 地 方 ， 
你 可 能 会 找到 某 些 小 众 的 软件 、 一 些 游戏 ， 甚 至 会 为 找 一 张 壁纸 花费 不 


少时 间 。 但 是 请 记 住 ， 你 是 用 Linux 来 工作 的 。 特 别 是 在 本 书 中 ， 你 是 用 
Linux 来 学 习 SLAM 的 ， 所 以 要 尽量 把 时 间 花 在 学 习 SLAM 上 。 


好 了 ， 我 们 选择 一 个 目录 ， 放 置 本 书 中 SLAM 程 序 的 代码 。 例 如 ， 可 
以 将 代码 放 到 家 目录 (Whome) 的 “slambook” 下 。 以 后 我 们 将 把 这 个 目录 
称 为 “代码 根 目录 ”。 同 时 ， 可 以 另外 选择 一 个 目录 ， 把 本 书 的 Git 代 码 复 
制 下 来 ， 方 便 做 实验 时 随时 对 照 。 本 书 的 代码 是 按 章节 划分 的 。 比 如 ， 
本 讲 的 代码 将 在 slambook/ch2 下 ， 下 一 讲 则 在 slambook/ch3 下 。 所 以 ， 现 
在 请 读者 进入 slambook/ch2 下 (你 应 该 会 新 建文 件 夹 并 进入 该 文件 夹 了 
吧 ) 。 


2.4.2 Hello SLAM 


我 们 从 最 基本 的 程序 开始 。 与 许多 计算 机 类 书籍 一 样 ， 我 们 来 书写 
一 个 HelloSLAM 程 序 。 不 过 在 做 这 件 事 之 前 ， 我 们 先 来 聊 聊 程序 是 什 
4 
人 。 


在 Linux 中 ， 程 序 是 一 个 具有 执行 权限 的 文件 。 它 可 以 是 一 个 脚本 ， 
也 可 以 是 一 个 二 进 制 文件 ， 不 过 我 们 不 限定 它 的 后 缀 名 (不 像 Windows 那 
样 需要 指定 成 .exe 文 件 ) 。 我 们 常用 的 cd、1s 等 命令 ， 就 是 位 于 /bin 目 录 下 
的 可 执行 文件 。 而 对 于 其 他 地 方 的 可 执行 程序 ， 只 要 它 有 可 执行 权限 ， 
那么 当 我 们 在 终端 中 输入 程序 名 时 ， 它 就 会 运行 。 在 C++ 编 程 时 ， 我 们 像 
下 面 这 样 用 编译 器 把 一 个 文本 文件 编译 成 可 执行 程序 。 


slambook/ch2/helloSLAM.cpp 


1 |#include <iostream> 


2 |using namespace std; 


4 |int main( int argc, char** argv ) 
5 | 

cout<<"Hello SLAM!"<<endl; 
return 0; 


8 |} 


这 是 一 个 非常 简单 的 程序 。 你 应 该 能 毫 不 费力 地 看 懂 它 ， 所 以 这 里 
不 多 加 解释 一 一 如 果实 际 情况 不 是 这 样 ， 请 你 先 补习 一 下 C++ 的 基本 知 
识 。 这 个 程序 只 是 把 一 个 字符 串 输 出 到 屏幕 上 而 已 。 你 可 以 用 文本 编辑 


器 gedit (或 Vim， 如 果 你 在 上 一 讲学 习 了 Vim) 输入 这 些 代 码 ， 并 保存 在 
上 面 列 出 的 路 径 下 。 现 在 ， 我 们 用 编译 器 g++ (g++ 是 一 个 C++ 编 译 器 ) 
把 它 编译 成 一 个 可 执行 文件 。 输 入 : 


1 | g++ helloSLAM.cpp | 


如 果 顺 利 ， 这 条 命令 应 该 没有 任何 输出 。 如 果 机 器 上 出 现 “command 
not found” 的 错误 信息 ， 说 明 你 可 能 还 没有 安装 g++， 请 用 如 下 命令 进行 
安装 : 


1 | sudo apt-get install g++ | 


如 果 出 现 别 的 错误 ， 请 再 检查 一 遍 刚才 的 程序 是 否 输入 正确 。 

刚才 这 条 编译 命令 把 helloSLAM.cpp 这 个 文本 文件 编译 成 了 一 个 可 执 
行程 序 。 我 们 检查 当前 目录 ， 会 发 现 多 了 一 个 a.out 文 件 ， 而 且 它 具有 执 
行 权限 (终端 里 颜色 不 同 ) 。 我 们 输入 ./a.out 即 可 运行 此 程序 : 


1 |% ./a.out 
2 |Hello SLAM! 


如 我 们 所 想 ， 这 个 程序 输出 “Hello SLAM!”， 告 诉 我 们 它 在 正确 运 


一 


{To 


请 回顾 一 下 我 们 之 前 做 的 事情 。 在 这 个 例子 中 ， 我 们 用 编辑 器 输入 
了 helloSLAM.cpp 的 源 代码 ， 然 后 调用 g++ 编 译 器 对 它 进行 编译 ， 得 到 了 
可 执行 文件 。g++ 默 认 把 源 文 件 编译 成 a.out 这 个 名 字 的 程序 《虽然 有 些 古 
怪 ， 但 是 可 以 接受 的 ) 。 如 果 我 们 愿意 ， 也 可 以 指定 这 个 输出 的 文件 名 
( 留 作 习题 ) 。 这 是 一 个 极其 简单 的 例子 ， 我 们 使 用 了 大 量 的 默认 参 
数 ， 几 乎 省 略 了 所 有 中 间 步 又 ， 为 的 是 给 读者 一 个 简洁 的 印象 (虽然 你 
可 能 没有 体会 到 ) 。 下 面 我 们 要 用 cmake 来 编译 这 个 程序 。 


2.4.3 “使 用 cmake 


理论 上 说 ， 任 意 一 个 C++ 程序 都 可 以 用 g++ 来 编译 。 但 当 程序 规模 越 
来 越 大 时 ， 一 个 工程 可 能 有 许多 个 文件 夹 和 源 文件 ， 这 时 输入 的 编译 命 
令 将 越 来 越 长 。 通 常 一 个 小 型 C++ 项 目 可 能 含有 十 几 个 类 ， 各 类 间 还 存在 
着 复杂 的 依赖 关系 。 其 中 一 部 分 要 编译 成 可 执行 文件 ， 另 一 部 分 编译 成 
库 文件 。 如 果 仅 靠 g++ 命 令 ， 我 们 需要 输入 大 量 的 编译 指令 ， 整 个 编译 过 
程 会 变 得 异常 烦琐 。 因 此 ， 对 于 C++ 项 目 ， 使 用 一 些 工程 管理 工具 会 更 加 
高 效 。 在 历史 上 工程 师 们 曾 使 用 make fi le 进行 自动 编译 ， 但 下 面 要 谈 的 
cmake 比 它 更 加 方便 。 并 且 ， 我 们 会 看 到 后 面 提 到 的 大 多 数 库 都 使 用 
cmake 来 管理 源 代码 。 


在 一 个 cmake 工 程 中 ， 我 们 会 用 cmake 命 令 生成 一 个 make fi le 文件 ， 
然后 ， 用 make 命 令 根据 这 个 make fi le 文件 的 内 容 编 译 整个 工程 。 读 者 可 
能 还 不 知道 make fi le 是 什么 东西 ， 不 过 没关系 ， 我 们 会 通过 例子 来 学 
习 。 仍 然 以 上 面 的 helloSLAM.cpp 为 例 ， 这 次 我 们 不 是 直接 使 用 g++， 而 
是 用 cmake 来 制作 一 个 工程 ， 然 后 再 编译 它 。 在 slambook/ch2/ 中 新 建 一 个 
CMakeLists.txt 文 件 ， 内 容 如 下 : 


kÀ slambook/ch2/CMakeLists.txt 


1 |# 声明 要 求 的 cmake 最 低 版 本 
2 | cmake_minimum_required( VERSION 2.8 ) 


4 |# 声明 一 个 cmake 工程 
5 | Project( HelloSLAM ) 


# 添加 一 个 可 执行 程序 
# 语法 : add_executable( 程序 名 源 代 码 文 件 ) 
add_executable( helloSLAM helloSLAM.cpp ) 


oy oo _ a 


CMakeLists.txt 文 件 用 于 告诉 cmake 我 们 要 对 这 个 目录 下 的 文件 做 什么 
事情 。CMake-Lists.txt 文 件 的 内 容 需 要 遵守 cmake 的 语法 。 这 个 示例 中 ， 
我 们 演示 了 最 基本 的 工程 : 指定 一 个 工程 名 和 一 个 可 执行 程序 。 根 据 注 
释 ， 读 者 应 该 理解 每 句 话 做 了 些 什么 。 

现在 ， 在 当前 目录 下 (slambook/ch2/) ， 调 用 cmake 对 该 工程 进行 分 
析 : 


1 | cmake . 


cmake 会 输出 一 些 编译 信息 ， 然 后 在 当前 目录 下 生成 一 些 中间 文 件 ， 
其 中 最 重要 的 就 是 MakeFilet00 。 由 于 MakeFile 是 自动 生成 的 ， 我 们 不 必修 
改 它 。 现 在 ， 用 make 命 令 对 工程 进行 编译 : 


1 |% make 

2 |Scanning dependencies of target helloSLAM 

3 | [100%] Building CXX object CMakeFiles/helloSLAM.dir/helloSLAM.cpp.o 
4 |Linking CXX executable helloSLAM 

5 | [100%] Built target helloSLAM 


编译 过 程 中 会 输出 一 个 编译 进度 。 如 果 顺 利通 过 ， 我 们 就 可 以 得 到 
在 CMakeLists.txt 中 声明 的 那个 可 执行 程序 helloSLAM。 执 行 它 : 


1 |% ./helloSLAM 
2 |Hello SLAM! 


因为 我 们 并 没有 修改 源 代 码 ， 所 以 得 到 的 结果 和 之 前 是 一 样 的 。 请 
读者 想 想 这 种 做 法 和 之 前 直接 使 用 g++ 编 译 的 区 别 。 这 次 我 们 用 cmake- 
make 的 做 法 ，cmake 过 程 处 理 了 工程 文件 之 间 的 关系 ， 而 make 过 程 实际 
调用 了 g++ 来 编译 程序 。 虽 然 这 个 过 程 中 多 了 调用 cmake 和 make 的 步骤 ， 
但 我 们 对 项 目的 编译 管理 工作 ， 从 输入 一 串 g++ 命 令 ， 变 成 了 维护 若干 个 
比较 直观 的 CMakeLists.txt 文 件 ， 这 将 明显 降低 维护 整个 工程 的 难度 。 比 
如 ， 如 果 想 新 增 一 个 可 执行 文件 ， 只 需 在 CMakeLists.txt 中 添加 一 行 
“add_executable” 命 令 即 可 ， 而 后 续 的 步骤 是 不 变 的 。cmake 会 帮 有 我 们 解决 
代码 的 依赖 关系 ， 而 无 须 输入 一 大 串 g++ 命 令 。 


现在 这 个 过 程 中 唯一 让 我 们 不 满 的 是 ，cmake 生 成 的 中 间 文 件 还 留 在 
我 们 的 代码 文件 当中 。 当 想 要 发 布 代码 时 ， 我 们 并 不 希望 把 这 些 中 间 文 
件 一 同 发 布 出 去 。 这 时 我 们 还 需要 把 它们 一 个 个 删除 ， 十 分 不 便 。 一 种 
更 好 的 做 法 是 让 这 些 中 间 文 件 都 放 在 一 个 中 间 目 录 中 ， 在 编译 成 功 后 ， 
把 这 个 中 间 目 录 删 除 即 可 。 所 以 ， 更 常见 的 编译 cmake 工 程 的 做 法 如 下 : 


1 |mkdir build 
2 |cd build 
3 |cmake .. 


4 |make 


我 们 新 建 了 一 个 中 间 文 件 夹 *build” ， 然 后 进入 build 文 件 夹 ， 通 过 
cmake.. 命 令 对 上 一 层 文 件 夹 ， 也 就 是 代码 所 在 的 文件 夹 进 行 编译 。 这 
样 ，cmake 产 生 的 中 间 文 件 就 会 生成 在 build 文 件 夹 中 ， 与 源 代码 分 开 。 当 
发 布 源 代码 时 ， 只 要 把 build 文 件 夹 删 掉 即 可 。 请 读者 自行 按照 这 种 方式 
对 ch2 中 的 代码 进行 编译 ， 然 后 调用 生成 的 可 执行 程序 (请 记得 把 上 一 步 
产生 的 中 间 文 件 删 掉 ) 。 


2.4.4 AE 


在 一 个 C++ 工 程 中 ， 并 不 是 所 有 代码 都 会 编译 成 可 执行 文件 。 只 有 市 
有 main 阔 数 的 文件 才 会 生成 可 执行 程序 。 而 另 一 些 代 码 ， 我 们 只 想 把 它 
们 打包 成 一 个 东西 ， 供 其 他 程序 调用 。 这 个 东西 叫 作 库 。 


一 个 库 往往 是 许多 算法 、 程 序 的 集合 ， 我 们 会 在 之 后 的 练习 中 接触 
到 许多 库 。 例 如 ，OpenCV 库 提供 了 许多 计算 机 视觉 相关 的 算法 ， 而 
Eigen 库 提供 了 和 矩阵 代数 的 计算 。 因 此 ， 我 们 要 学 习 如 何 用 cmake 生 成 
库 ， 并 且 使 用 库 中 的 冰 数 。 现 在 书写 如 下 libHelloSLAM.cpp 文 件 。 


内 slambook/ch2/libHelloSLAM.cpp 


1 | // 这 是 一 个 库 文件 

2 |#include <iostream> 

3 |using namespace std; 

4 |void printHello() 

5 |{ 

cout<<"Hello SLAM"<<end1; 


7 |} 


这 个 库 提 供 了 一 个 printHello 国 数 ， 调 用 此 函数 将 输出 一 条 信息 。 但 
是 它 没有 main 孙 数 ， 这 意味 着 这 个 库 中 没有 可 执行 文件 。 我 们 在 
CMakeLists.txt 里 加 上 如 下 内 容 : 


1 | add_library( hello libHelloSLAM.cpp ) 


这 条 命令 告诉 cmake， 我 们 想 把 这 个 文件 编译 成 一 个 叫 作 “hello” 的 
库 。 然 后 ， 和 上 面 一 样 ， 使 用 cmake 编 译 整个 工程 : 


1 | cd build 


2 |cmake .. 


3 | make 


这 时 ， 在 build 文 件 夹 中 就 会 生成 一 个 libhello.a 文 件 ， 这 就 是 我 们 得 到 
的 库 。 


在 Linux 中 ， 库 文件 分 成 静态 库 和 共享 库 两 种 "2 。 静 态 库 以 .a 作 为 后 
缀 名 ， 共 享 库 以 .so 结尾 。 所 有 库 都 是 一 些 阔 数 打 包 后 的 集合 ， 差 别 在 于 
静态 库 每 次 被 调用 都 会 生成 一 个 副本 ， 而 共享 库 则 只 有 一 个 副本 ， 更 省 
空间 。 如 果 想 生成 共享 库 而 不 是 静态 库 ， 只 需 使 用 以 下 语句 即 可 。 


1 | add_library( hello_shared SHARED libHelloSLAM.cpp ) 


此 时 得 到 的 文件 就 是 libhello_shared.so 了 。 


库 文 件 是 一 个 压缩 包 ， 里 面 有 编译 好 的 二 进 制 函数 。 不 过 ， 如 果 仅 
有 .a 或 .so 库 文 件 ， 那 么 我 们 并 不 知道 里 面 的 负数 到 底 是 什么 ， 调 用 的 形式 
又 是 什么 样 。 为 了 让 别人 (或 者 自己 ) 使 用 这 个 库 ， 我 们 需要 提供 一 个 
头 文件 ， 说 明 这 些 库 里 都 有 些 人 什么。 因此， 对 于 库 的 使 用 者 ， 只 要 拿 到 
了 头 文件 和 库 文 件 ， 就 可 以 调用 这 个 库 了 。 下 面 编写 libhello 的 头 文件 。 


kÀ slambook/ch2/libHelloSLAM.h 


1 | #ifndef LIBHELLOSLAM_H_ 
2 | #define LIBHELLOSLAM_H_ 
3 | void printHello(); 

4 | #endif 


这 样 ， 根 据 这 个 文件 和 我 们 刚才 编译 得 到 的 库 文 件 ， 就 可 以 使 用 
printHelloRK XS. 下 面 写 一 个 可 执行 程序 来 调用 这 个 简单 的 函数 : 


内 slambook/ch2/useHello.cpp 


1 | #include "libHelloSLAM.h" 


N 


int main( int argc, char** argv ) 
3 |{ 
4 printHello(); 


return 0; 


6 |} 


然后 ， 在 CMakeLists.txt 中 添加 一 个 可 执行 程序 的 生成 命令 ， 链 接 到 
刚才 使 用 的 库 上 : 


1 |add_executable( useHello useHello.cpp ) 


2 | target_link_libraries( useHello hello_shared ) 


通过 这 两 行 语句 ，useHello 程 序 就 能 顺利 使 用 hello_shared 库 中 的 代码 
了 。 这 个 小 例子 演示 了 如 何 生 成 并 调用 一 个 库 。 请 注意 ， 对 于 他 人 提供 
的 库 ， 我 们 也 可 用 同样 的 方式 对 它们 进行 调用 ， 整 合 到 自己 的 程序 中 。 


除了 已 经 演示 的 功能 之 外 ，cmake 还 有 许多 语法 和 选项 ， 这 里 不 一 一 
列举 。 习 题 中 包含 了 一 些 cmake 的 阅读 材料 ， 感 兴趣 的 读者 可 自行 阅读 。 
现在 ， 简 单 回顾 一 下 我 们 之 前 做 了 哪些 事 : 

1. 首 先 ， 程 序 代 码 由 头 文 件 和 源 文件 组 成 。 

2. 带 有 main 了 因数 的 源 文 件 编译 成 可 执行 程序 ， 其 他 的 编译 成 库 文 件 。 

3. 如 果 可 执行 程序 想 调用 库 文 件 中 的 遂 数 ， 它 需要 参考 该 库 提供 的 头 
文件 ， 以 明白 调用 的 格式 。 同 时 ， 要 把 可 执行 程序 链接 到 库 文件 上 。 

这 几 个 步骤 应 该 是 简单 清楚 的 ， 但 实际 操作 中 你 可 能 会 遇 上 一 些 问 
题 。 比 如 说 ， 如 果 代 码 里 引用 了 库 的 函数 ， 但 乐 了 把 程序 链接 到 库 上 ， 
会 发 生 什 么 呢 ? 请 试 试 把 CMake-Lists.txt 中 的 链接 部 分 去 掉 ， 看 看 会 发 生 
什么 情况 。 你 能 看 懂 cmake 报 告 的 错误 消息 吗 ? 


2.4.5 ”使 用 IDE 


最 后 ， 我 们 来 谈 谈 如 何 使 用 集成 开发 环境 (Integrated Development 
Environment, IDE) 。 前 面 的 编程 完全 可 以 用 一 个 简单 的 文本 编辑 器 来 
完成 。 然 而 ， 你 可 能 需要 在 各 个 文件 间 跳 来 跳 去 ， 查 询 某 个 函数 的 声明 
和 实现 。 当 文件 很 多 时 ， 这 仍然 很 烦琐 。IDE 为 开发 者 提供 了 跳 转 、 补 
全 、 断 点 调试 等 很 多 方便 的 功能 ， 所 以 ， 我 们 建议 读者 选择 一 个 IDE 进 行 
开发 。 

Linux 下 的 IDE 有 很 多 种 。 虽 然 与 Windows 下 的 Visual Studio 还 有 一 些 
差 跑 ， 不 过 支持 C++ 开发 的 也 有 好 几 种 ， 例 如 : Eclipse、QtCreator、 
Code::Blocks、Clion ， 等 等 。 同 样 ， 我 们 不 强制 读者 使 用 某 种 特定 的 
IDE， 而 仅 给 出 我 们 的 建议 。 我 们 使 用 的 是 Kdevelop (WH2-12) . Cx 
一 个 免费 软件 ， 在 Ubuntu 的 源 中 提供 了 ， 这 意味 着 你 可 以 用 apt-get 来 安装 
它 。Kdevelop 的 优点 列举 如 下 : 


1. 支 持 cmake 工 程 。 


2. 对 C++ 支 持 较 好 (包括 11 标 准 ) 。 有 高 亮 、 跳 转 、 补 全 等 功能 。 能 
目 动 排版 代码 。 


3. 能 方便 地 看 到 各 个 文件 和 目录 树 。 
4. 有 一 键 编译 、 断 点 调试 等 功能 。 
5. 无 须 付费 。 


default: ch2 - [ ch2/libHelloSLAM.cpp ] - KDevelop 
e is Debug 
9 5 CMakeLists.txt 其 libHelloSLAM.h 其 libHelloSLAM.cpp % | useHello.cpp 其 


#include <iostream> 
using namespace std; 


void printHello() 
wt 
cout<<"Hello SLAM"<<end1; 


CONDUAWNH 


} 


b libHelloSLAM.h 
& useHello.cpp 
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Build files have been written to: /home/xiang/slambook/ch2/buil 
[ 2596] [ 50%] Bulk target hello 


Built target hel 
[75%] Built ta lo_shared 
Scannin e ftarg 


encies ol 
[10096] OX object 
Linking OX executable useHello 
[100%] Bullt target useHello 
+t 已 完成 +++ 
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图 2-12 Kdevelop R Ho 


基本 上 ， 我 们 对 一 个 IDE 的 功能 要 求 它 都 具备 ， 所 以 读者 不 妨 党 试 一 
下 。 有 时 候 你 会 碰 到 一 些 问题 ， 例 如 对 某 些 模板 类 的 解析 啊 应 比较 缓慢 
等 ， 确 实 它 还 不 够 完善 。 不 过 相 比 于 其 他 IDE， 它 是 不 错 的 。 


Kdevelop 原生 文 持 cmake 工 程 。 上 有 具体 做 法 是 ， 在 终端 建立 
CMakeLists.txt 后 ， 用 Kdevelop 中 的 “工程 ~ 打开 /导入 工程 > 打开 
CMakeLists.txt. 软件 会 询问 你 几 个 问题 ， 并 且 默 认 建立 一 个 build 文 件 
夹 ， 帮 你 调用 刚才 的 cmake 和 make 命 令 。 只 要 按 下 快捷 键 F8， 这 些 都 可 以 
自动 完成 。 图 2-12 的 下 面部 分 就 显示 了 编译 信息 。 


我 们 把 适应 IDE 的 任务 交 给 读者 自己 来 完成 ， 而 并 不 打算 在 书 中 进行 
详细 说 明 。 如 果 你 是 从 Windows 转 过 来 的 ， 会 觉得 它 的 界面 与 Visual 
C++ 或 Visual Studio 挺 相似 。 请 用 Kdevelop 打 开 刚 才 的 工程 然后 进行 编 
译 ， 看 看 它 输出 什么 信息 。 相 信 你 会 觉得 比 打开 终端 更 方便 一 些 。 


不 过 ， 本 节 重 点 想 讲 的 是 如 何在 IDE 中 进行 调试 。 在 Windows 下 编程 
的 同学 多 半 会 有 在 Visual Studio 下 断 点 调试 的 经 历 。 不 过 在 Linux 中 ， 默 认 
的 调试 工具 gdb 只 提供 了 文本 界面 ， 对 新 手 来 讲 不 太 方 便 。 有 些 IDE 提 供 
了 断 点 调试 功能 〈 底 层 仍 旧 是 gdb) ，Kdevelop 就 是 其 中 之 一 。 要 使 用 
Kdevelop 的 断 点 调试 功能 ， 你 需要 完成 以 下 几 件 事 : 


1. 在 CMakeLists.txt 中 把 工程 调 为 Debug 编 译 模 式 。 


2. 告 诉 Kdevelop 你 想 运行 哪个 程序 。 如 果 有 参数 ， 也 要 配置 它 的 参数 
和 工作 目录 。 


3. 进 入 断 点 调试 界面 ， 就 可 以 单 步 运行 ， 看 到 中 间 变 量 的 值 了 。 
第 一 步 ， 在 CMakeLists.txt 中 加 入 下 面 的 命令 来 设置 编译 模式 : 


1 |set( CMAKE_BUILD_TYPE "Debug" ) 


cmake 自 带 一 些 编译 相关 的 内 部 变量 ， 它 们 可 以 对 编译 过 程 进行 更 精 
细 的 控制 。 对 于 编译 类 型 ， 通 党 有 调试 用 的 Debug 模 式 与 发 布 用 的 Release 
模式 。 在 Debug 模 式 中 ， 程 序 运行 较 慢 ， 但 可 以 进行 断 点 调试 ， 而 Release 
模式 则 速度 较 快 ， 但 没有 调试 信息 。 我 们 把 程序 设置 成 Debug 模 式 ， 就 能 
放置 断 点 了 。 接 下 来 ， 告诉 Kdevelop 你 想 启 动 哪个 程序 。 


第 二 步 ， 打 开 “ 运 行 > 配置 启动 器 *， 然 后 单 击 左 侧 的 “Add New 应 
用 程序 ”。 在 这 一 步 中 ， 我 们 的 任务 是 告诉 Kdevelop 想 要 启动 哪 一 个 程 
序 。 如 图 2-13 所 示 ， 既 可 以 直接 选择 一 个 cmake 的 工程 目标 (也 就 是 我 们 
用 add_executable 指 令 构 建 的 可 执行 程序 ) ， 也 可 以 直接 指向 一 个 二 进 制 
文件 。 建 议 使 用 第 二 种 方式 ， 根 据 我 们 的 经 验 ， 这 样 更 少 出 现 问题 。 


+ Add New...v| | 一 Remove Selected Editing 应 用 程序 :新建 应 用 程序 启动 器 
可 执行 文件 
工程 目标 : @ Och2/useHello 


vio 新 建 应 用 程序 启动 器 = 
RITE : 
‘mt 可 执行 文 
行为 
Sm: 
工作 目录 : 
环境 : 
使 用 外 部 终端 : 
依赖 
动作 : | 无 动作 


目标 : 


图 2-13 ”启动 器 设置 界面 。 


在 第 二 栏 里 ， 可 以 设置 程序 的 运行 参数 和 工作 目录 。 有 时 程序 是 有 
运行 参数 的 ， 它 们 会 作为 main 函 数 的 参数 被 传 入 。 如 果 没 有 则 可 以 留 
空 ， 对 于 工作 目录 亦 是 如 此 。 配 置 好 这 两 项 后 ， 可 以 单 击 ^“OK” 按 钮 保存 
配置 结果 。 


刚才 这 几 步 我 们 配置 了 一 个 应 用 程序 的 启动 项 。 对 于 每 一 个 启动 
项 ， 我 们 可 以 单 击 “Execute” 按 钮 直接 启动 这 个 程序 ， 也 可 单 击 “Debug” 按 
钮 对 它 进 行 断 点 调试 。 读 者 可 以 试 着 单 击 “Execute” 按 钮 ， 查 看 输出 的 结 
果 。 现 在 ， 为 了 调试 这 个 程序 ， 单 击 printHello 那 行 的 左 侧 ， 增 加 一 个 断 
操 。 然 后 ， 单 击 “Debug” 按 钮 ， 程 序 会 停留 在 断 点 处 等 待 ， 如 图 2-14 所 


小 o 


图 2-14 调试 界面 。 


调试 时 ，Kdevelop 会 切换 到 调试 模式 ， 界 面 会 发 生 一 点 变化 。 在 断 
点 处 ， 可 以 用 单 步 运 行 (F10 键 ) 、 单 步 跟 进 (F11 键 ) 、 单 步 跳出 (F12 
键 ) 功能 控制 程序 的 运行 。 同 时 ， 可 以 点 开 左 侧 的 界面 ， 查 看 局 部 变量 
的 值 。 或 者 选择 “停止 "按钮 ， 结 束 调试 。 调 试 结束 后 ，Kdevelop 会 回 到 正 
常 的 开发 界面 。 

现在 你 应 该 熟悉 了 整个 断 点 调试 的 流程 。 今 后 ， 如 果 在 程序 运行 阶 
段 发 生 了 错误 ， 导 致 程序 骨 溃 ， 就 可 以 用 断 点 调试 确定 出 错 的 位 置 ， 然 
后 加 以 修正 53l 。 

习题 


1. 阅 读 文献 [和 [14]， 你 能 看 懂 其 中 的 内 容 吗 ? 


2.+* 阅 读 SLAM 的 综述 文献 ， 例 如 [9,15,16,17,18] 等 。 这 些 文 献 关 于 
SLAM 的 看 法 与 本 书 有 何 异 同 ? 


3.g++ 命 令 有 哪些 参数 ”怎么 填写 参数 可 以 更 改 生成 的 程序 文件 名 ? 
4. 使 用 build 文 件 夹 来 编译 你 的 cmake 工 程 ， 然 后 在 Kdevelop 中 试 试 。 


5. 刻 意 在 代码 中 添加 一 些 语法 错误 ， 看 看 编译 会 生成 什么 样 的 信息 。 
你 能 看 懂 g++ 的 错误 信息 吗 ? 


6. 如 果 忘 了 把 库 链接 到 可 执行 程序 上 ， 编 译 会 报错 吗 ? 报 什么 样 的 
错 ? 


7.* 阅 读 《cmake 实 践 》”》， 了 解 cmake 的 其 他 语法 。 


8.* 完 善 hello SLAM 小 程序 ， 把 它 做 成 一 个 小 程序 库 ， 安 装 到 本 地 硬 
盘 中 。 然 后 ， 新 建 一 个 工程 ， 使 用 find_package 找 这 个 库 并 调用 。 


9.* 寻找 其 他 cmake 教 学 材料 ， 深 入 了 解 cmake ， 例 如 
https://github.com/TheErk/CMake-tutorialo 


10. 找 到 Kdevelop 的 官方 网 站 ， 看 看 它 还 有 哪些 特性 。 你 都 用 上 了 
吗 ? 


11. 如 果 在 上 一 讲学 习 了 Vim， 请 试 试 Kdevelop 的 Vim 编 辑 功 能 。 


[1] 不 过 现实 中 我 们 都 会 有 一 个 大 概 的 范围 ， 例 如 室内 和 室外 的 区 分 。 

[2] 画 成 单 目 会 比较 吓人 。 

[3] 数学 上 的 原因 将 会 在 视觉 里 程 计 一 讲 中 解释 。 

[4] 你 可 以 用 手机 录 个 小 视频 试 试 。 

[5] 更 多 时 候 称 为 后 端 (Back End) 。 由 于 主要 使 用 的 是 优化 方法 ， 故 称 为 后 端 优化 。 
[6] https://en. wikipedia.org/wiki/A*_search_algorithm 

[7] 在 本 书 中 ， 我 们 以 “位 姿 " 这 个 词 表 示 “ 位 置 ? 加 上 “姿态 ”。 


[8] 我 们 以 后 称 它 为 位 姿 (Pose) ， 以 与 位 置 进行 区 别 。 我 们 说 的 位 姿 ， 包 含 了 旋转 (Rotation) 
和 平移 (Translation) 。 


[9] 感谢 TUNA 同 学 们 的 维护 ! 


[10] 大 多 数 人 第 一 次 看 到 Ubuntu 都 觉得 很 漂亮 。 


[11] MakeFile 是 一 个 自动 化 编译 的 脚本 ， 读 者 现在 可 以 将 它 理 解 成 一 系统 自动 生成 的 编译 指令 ， 
而 无 须 理会 其 内 容 。 


[12] 你 多 半 猜 错 了 ， 它 并 不 叫 作 动态 库 。 
[13] 而 不 是 直接 给 我 们 发 邮件 询问 怎么 处 理 遇 到 的 问题 。 


第 3 讲 三维 空间 刚体 运动 


主要 目标 


1. 理 解 三 维 空间 的 刚体 运动 描述 方式 : 旋转 矩阵 、 变 换 和 矩 阵 、 四 元 数 
和 欧 拉 角 。 


2. 掌 握 Eigen 库 的 矩阵 、 几 何 模 块 使 用 方法 。 


在 上 一 讲 中 ， 我 们 讲解 了 视觉 SLAM 的 框架 与 内 容 。 本 讲 将 介绍 视觉 
SLAM 的 基本 问题 之 一 : 一 个 刚体 在 三 维 空间 中 的 运动 是 如 何 描述 的 。 
我 们 当然 知道 这 由 一 次 旋转 加 一 次 平移 组 成 。 平 移 确实 没有 太 大 问题 ， 
但 旋转 的 处 理 是 件 麻 烦 事 。 我 们 将 介绍 旋转 矩阵、 四 元 数 、 欧 拉 角 的 意 
义 ， 以 及 它们 是 如 何 运算 和 转换 的 。 在 实践 部 分 ， 我 们 将 介绍 线性 代数 
库 Eigen。 它 提供 了 C++ 中 的 矩阵 运算 ， 并 且 它 的 Geometry 模 块 还 提供 了 
四 元 数 等 刚体 运动 的 描述 。Eigen 的 优化 非常 完善 ， 但 是 它 的 使 用 方法 有 
一 些 特殊 的 地 方 ， 我 们 会 在 程序 中 介绍 。 
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3.1 ”旋转 和 矩阵 
3.1.1 RAR, BR 


我 们 日 常生 活 的 空间 是 三 维 的 ， 因 此 我 们 生来 就 习惯 于 三 维 空间 的 
运动 。 三 维 空间 由 3 个 轴 组 成 ， 所 以 一 个 空间 点 的 位 置 可 以 由 3 个 坐标 指 
定 。 不 过 ， 我 们 现在 要 考虑 刚体 ， 它 不 光 有 位 置 ， 还 有 自身 的 姿态 。 相 
机 也 可 以 看 成 三 维 空间 的 刚体 ， 于 是 位 置 是 指 相 机 在 空间 中 的 哪个 地 
方 ， 而 姿态 则 是 指 相 机 的 朝向 。 结 合 起 来 ， 我 们 可 以 说 ,“ 相 机 正 处 于 空 
间 (0, 0, 0) 点 处 ， 朝 向 正 前 方 ” 这 样 的 话 。 但 是 这 种 自然 语言 很 烦琐 ， 我 们 
更 喜欢 用 数学 语言 来 描述 它 。 

我 们 从 最 基本 的 内 容 讲 起 : 扣 和 向 量 。 点 的 几何 意义 很 容易 理解 。 
向 量 是 什么 呢 ? 它 是 线性 空间 中 的 一 个 元 素 ， 可 以 把 它 想 象 成 从 原点 指 
向 某 处 的 一 个 箭头 。 需 要 提醒 读者 的 是 ， 请 不 要 把 向 量 与 它 的 坐标 两 个 
概念 混淆 。 一 个 向 量 是 空间 当中 的 一 样 东西 ， 比 如 说 ao 。 这 里 a 并 不 是 和 
若干 个 实数 相关 联 的 。 只 有 当 我 们 指定 这 个 三 维 空间 中 的 某 个 坐标 系 
时 ， 才 可 以 谈论 该 向 量 在 此 华 标 系 下 的 坐标 ， 也 就 是 找到 若干 个 实数 对 
应 这 个 向 量 。 例 如 ， 三 维 空间 中 的 某 个 向 量 的 坐标 可 以 用 Rs 当中 的 3 个 数 
来 描述 。 某 个 点 的 坐标 也 可 以 用 Rs RIA. AIRE? 如 果 我 们 确定 
了 一 个 坐标 系 ， 也 就 是 一 个 线性 空间 的 基 (e | ,e , ,e ;)， 那 么 就 可 以 谈论 向 
Ba 在 这 组 基 下 的 坐标 了 : 


a = [€1,€2,e3] | ap | = alel 十 aze2 + ages. (3.1) 


所 以 坐标 的 具体 取 值 ， 一 是 和 向 量 本 身 有 关 ， 二 是 和 坐标 系 的 选取 
有 关 。 坐 标 系 通 常 由 3 个 正 交 的 坐标 轴 组 成 《尽管 也 可 以 有 非 正 交 的 ， 但 
实际 中 很 少见 ) 。 例 如 ， 给 定 x 和 y 轴 时 ，z 轴 就 可 以 通过 右手 (MA 
F) 法 则 由 xxy 定义 出 来 。 根 据 定 义 方 式 的 不 同 ， 坐 标 系 又 分 为 左手 系 和 
右手 系 。 左 手 系 的 第 3 个 轴 与 右手 系 方向 相反 。 就 经 验 来 讲 ， 人 们 更 习惯 
使 用 右手 系 ， 尽 管 也 有 一 部 分 程序 库 仍 使 用 左手 系 。 


根据 基本 的 线性 代数 知识 ， 我 们 可 以 谈论 向 量 与 向 量 ， 以 及 向 量 与 
效 之 间 的 运算 ， 例 如 数 乘 、 加 法 、 减 法 、 内 积 、 外 积 等 。 数 乘 和 四 则 运 
算 都 是 相当 基本 的 内 容 ， 这 里 不 再 资 述 。 内 外 积 对 读者 来 说 可 能 有 些 阳 
生 ， 这 里 给 出 它们 的 运算 方式 。 对 于 ab E R? ， 内 积 可 以 写成 : 


3 
a-b=a'b= X aibi = |a| |b| cos (a, b) . (3.2) 
i=1 


内 积 可 以 描述 向 量 间 的 投影 关系 。 而 外 积 则 是 这 个 样子 : 


i 7 k Q203 = Q3b2 0 一 Q3 ag 
A 
axb= aj a2 Q3 = | asb1 — aıb3 = a3 0 一 Q1 b=a^b. (3.3) 
by bo bs a,b 一 a2b1 —ag ay 0 


外 积 的 方向 垂直 于 这 两 个 向 量 ， 大 小 为 lallbl sn (b> ， 是 两 个 向 
量 张 成 的 四 边 形 的 有 向 面积 。 对 于 外 积 ， 我 们 引入 了 “^ 符号 ， 把 a 写成 一 
个 矩阵 。 事 实 上 是 一 个 反对 称 和 矩阵 (Skew-symmetric) ， 你 可 以 将 ^ 记 成 
一 个 反对 称 符号 。 这 样 就 把 外 积 axb 写成 了 和 矩阵 与 向 量 的 乘法 ao^45 ， 把 
它 变 成 了 线性 运算 。 这 个 符号 将 在 后 文 经 常用 到 ， 请 记 住 它 。 外 积 只 对 
三 维 向 量 存在 定义 ， 我 们 还 能 用 外 积 表示 向 量 的 旋转 。 


为 什么 外 积 可 以 表示 旋转 呢 ? 


考虑 两 个 不 平行 的 向 量 a,b ， 我 们 要 描述 从 a Bb 之 间 是 如 何 旋 转 
的 ， 如 图 3-1 所 示 。 我 们 可 以 用 一 个 向 量 来 描述 三 维 空间 中 两 个 向 量 的 旋 
转 关 系 。 在 右手 法 则 下 ， 我 们 用 右手 的 4 个 指头 从 a 转向 b ， 大 拇指 朝向 
就 是 旋转 向 量 的 方向 ， 事 实 上 也 是 axb 的 方向 。 它 的 大 小 则 由 a 和 b WX 
角 决 定 。 通 过 这 种 方式 ， 我 们 构造 了 从 a 到 b 的 一 个 旋转 向 量 。 这 个 向 量 
同样 位 于 三 维 空间 中 ， 在 此 华 标 系 下 ， 可 以 用 3 个 实数 来 摘 述 。 


W 
Z 
X b 
y 
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Z 
HER KER 旋转 表示 


图 3-1 左右 手 系 的 区 别 与 向 量 间 的 旋转 。a Bb 的 旋转 可 以 由 向 量 w 来 描述 。 


3.1.2 ”坐标 系 间 的 欧 氏 变换 


与 向 量 间 的 旋转 类 似 ， 同 样 可 以 描述 两 个 坐标 系 之 间 的 旋转 天 系 ， 
再 加 上 平移 ， 统 称 为 坐标 系 之 间 的 变换 关系 。 在 机 器 人 的 运动 过 程 中 ， 
常见 的 做 法 是 设 定 一 个 惯性 坐标 系 《或 者 叫 世界 坐标 系 ) ， 可 以 认为 它 
是 固定 不 动 的 ， 例 如 图 3-2 中 的 xy ,yy ,zw 定义 的 坐标 系 。 同 时 ， 相 机 或 机 
器 人 则 是 一 个 移动 坐标 系 ， 例 如 x。 ,y。 ,z 定义 的 坐标 系 。 我 们 可 能 会 问 : 
相机 视野 中 某 个 向 量 p ， 它 的 坐标 为 p. ， 而 在 世界 坐标 系 下 看 ， 它 的 坐标 
pv,。 这 两 个 坐标 之 间 是 如 何 转换 的 呢 ? 这 时 ， 融 需要 先 得 到 该 点 针对 机 


器 人 坐标 系 的 坐标 值 ， 再 根据 机 器 人 位 资 转换 到 世界 坐标 系 中 ， 这 个 转 
换 关系 由 一 个 矩阵 T 来 描述 ， 如 图 3-2 所 示 。 
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Xy =Q p 
图 3-2 ”坐标 变换 。 对 于 同一 个 向 量 p ， 它 在 世界 坐标 系 下 的 坐标 p ,和 在 相机 坐标 系 下 的 坐 
标 p .是 不 同 的 。 这 个 变换 关系 由 坐标 系 间 的 变换 矩阵 了 来 描述 。 


相机 运动 是 一 个 刚体 运动 ， PEA eee) oe 
长 度 和 夹 角 都 不 会 发 生变 化 。 这 种 变换 称 为 欧 氏 变换 。 想 象 你 把 手机 抛 
到 空中 ， 在 它 洛 地 摔 碎 之 前 ， 只 可 能 有 空间 位 置 和 姿态 的 不 同 ， 而 它 自 
己 的 长 度 、 各 个 面 的 角度 等 性 质 不 会 有 任何 变 化 。 这 样 一 个 欧 氏 变换 由 
一 个 旋转 和 一 个 平移 两 部 分 组 成 。 首 先 来 考虑 旋转 。 我 们 设 录 个 单位 正 
交 基 (ei, eo, e@3) 经 过 一 次 旋转 变 成 了 ( (ete%,e9)。 那 么 ， 对 于 同一 个 向 量 a 
(注意 该 向 量 并 没有 随 着 坐标 系 的 旋转 而 发 生 运动 ) ， 它 在 两 个 坐标 系 
下 的 坐标 为 [a1,az,as]" Hlal aras". 根据 坐标 的 定义 ， 有 : 


[e1,€2,e3] | ay | = e1, es, e3] oe le (3.4) 


为 了 描述 两 个 坐标 之 间 的 关系 ， 我 们 对 上 述 等 式 的 左右 两 边 同时 左 


ei 


R| ot |, 那么 左边 的 系数 就 变 成 了 单位 矩阵 ， 所 以 : 


Tat Taz To/ 


a4 Eier €163 €163 ay 

A / 
a2 | = | ede, ele, ele’ a, | 5 Ra. (3.5) 
a3 ere, ele, elel a’ 


我 们 把 中 间 的 矩阵 拿 出 来 ， 定 义 成 一 个 矩阵 R 。 这 个 矩阵 由 两 组 基 之 
间 的 内 积 组 成 ， 刻 画 了 旋转 前 后 同一 个 向 量 的 坐标 变换 关系 。 只 要 旋转 
是 一 样 的 ， 那 么 这 个 和 矩阵 也 是 一 样 的 。 可 以 说 ， 和 矩阵 R 描述 了 旋转 本 身 。 
因此 又 称 为 旋转 矩阵 。 


旋转 矩阵 有 一 些 特 别 的 性 质 。 事 实 上 ， 它 是 一 个 行列 式 为 1 的 正 交 答 


阵 趾 。 反 之 ,行列 式 为 1 的 正 交 和 矩阵 也 是 一 个 旋转 和 矩阵。 所以， 可 以 把 旋 
转 和 矩阵 的 集合 定义 如 下 : 


SO(n) = {R € R”*"|RR' = T, det(R) = 1}. (3.6) 


SO(n ) 是 特殊 正 交 群 (Special Orthogonal Group) 的 意思 。 我 们 把 解 
释 “ 群 ”的 内 容留 到 下 一 讲 。 这 个 集合 由 mn” 维 空间 的 旋转 矩阵 组 成 ， 特 别 
地 ，SO(3) 就 是 三 维 空间 的 旋转 了 。 通 过 旋转 矩阵 ， 我 们 可 以 直接 谈论 两 
个 坐标 系 之 间 的 旋转 变换 ， 而 不 用 再 从 基 开 始 谈 起 。 换 句 话 说 ， 旋 转 矩 
阵 可 以 描述 相机 的 旋转 。 


由 于 旋转 矩阵 为 正 交 和 矩阵 ， 它 的 逆 ( 即 转 置 ) 描述 了 一 个 相反 的 旋 
转 。 按 照 上 面 的 定义 方式 ， 有 : 


a’ = Rta = R'a. (3.7) 


显然 RT 刻画 了 一 个 相反 的 旋转 。 

在 欧 氏 变换 中 ， 除 了 旋转 之 外 还 有 平移 。 考 虑 世界 坐标 系 中 的 向 量 a 
， 经 过 一 次 旋转 (用 R 描述 ) 和 一 次 平移 t 后， 得 到 了 a ， 那 么 把 旋转 和 
平移 合 到 一 起 ， 有 : 


a’ = Ra+t. (3.8) 


其 中 ,上 称 为 平移 向 量 。 相 比 于 旋转 ， 平 移 部 分 只 需 把 这 个 平移 量 加 
到 旋转 之 后 的 坐标 上 ， 显 得 非常 简洁 。 通 过 上 式 ， 我 们 用 一 个 旋转 和 矩阵 R 
和 一 个 平移 向 量 t 完整 地 描述 了 一 个 欧 氏 空间 的 坐标 变换 关系 。 


3.1.3 ”变换 矩阵 与 齐 次 坐标 


TL (3.8) 完整 地 表达 了 欧 氏 空间 的 旋转 与 平移 ， 不 过 还 存在 一 个 小 
问题 : 这 里 的 变换 关系 不 是 一 个 线性 关系 。 假 设 我 们 进行 了 两 次 变换 : R 
ot, 和 R, ,t,， 满 足 : 


b= Ria +4 ti, c= Rəb + tə. 


但 是 从 a 到 c 的 变换 为 
c= Ro (Ria + tı) + to. 


这 样 的 形式 在 变换 多 次 之 后 会 过 于 和 复杂。 因此， 我 们 要 引入 齐 次 坐 
标 和 变换 矩阵 重 写 式 (3.8): 
sz| ?| (3.9) 
1 


加 

这 是 一 个 数学 技巧 : 我 们 在 一 个 三 维 向 量 的 末尾 添加 1， 将 其 变 成 了 
四 维 向量 ， 称 为 齐 次 坐标 。 对 于 这 个 四 维 向 量 ， 我 们 可 以 把 旋转 和 平移 
写 在 一 个 和 矩阵 里 面 ， 使 得 整个 关系 变 成 线性 关系 。 该 式 中 ， 和 矩 阵 了 称 为 变 
FRERE (Transform Matrix) 。 我 们 暂时 用 4 表示 a 的 齐 次 坐标 。 

稍微 说 一 下 齐 次 坐标 。 它 是 射影 几何 里 的 概念 。 通 过 添加 最 后 一 
维 ， 我 们 用 4 个 实数 描述 了 一 个 三 维 向 量 ， 这 显然 多 了 一 个 自由 度 ， 但 允 
许 我 们 把 变换 写成 线性 的 形式 。 在 齐 次 坐标 中 ， 某 个 点 x 的 每 个 分 量 同 乘 
一 个 非 零 常数 k 后， 仍然 表示 同一 个 点 。 因 此 ， 一 个 点 的 具体 坐标 值 不 
是 唯一 的 。 如 [1, 1,1, 1 和 [2, 2, 2, 2J? 是 同一 个 点 。 但 当 最 后 一 项 不 为 零 
时 ， 我 们 总 可 以 把 所 有 坐标 除 以 最 后 一 项 ， 强 制 最 后 一 项 为 1， 从 而 得 到 
一 个 点 唯一 的 坐标 表示 (也 就 是 转换 成 非 齐 次 坐标 ) : 


Rt 
07 1 


& = |z, y, z, w|" = [x/w,y/w,z/w, 1)". (3.10) 


这 时 ， 忽 略 掉 最 后 一 项 ， 这 个 点 的 坐标 和 欧 氏 空间 就 是 一 样 的 。 依 
靠 齐 次 坐标 和 变换 矩阵， 两 次 变换 的 累加 就 可 以 有 很 好 的 形式 : 


b=T\4,6=TMb 6e=TDTa. (3.11) 


但 是 区 分 齐 次 和 非 齐 次 坐标 的 符号 令 我 们 感到 厌烦 。 在 不 引起 歧义 
的 情况 下 ， 以 后 我 们 就 直接 把 它 写 成 b =Ta 的 样子 ， 默 认 其 中 是 齐 次 坐标 
了 。 


关于 变换 矩阵 7 ， 它 具有 比较 特别 的 结构 : ALAM, All 
为 平移 向 量 ， 左 下 角 为 0 向 量 ， 右 下 角 为 1。 这 种 矩阵 又 称 为 特殊 欧 氏 群 
(Special Euclidean Group) : 


SE(3) = fr = 


与 SO(3) 一 样 ， 求 解 该 矩阵 的 逆 表 示 一 个 反 向 的 变换 : 


t 4x4 3 
E R***|R € SO(3),t Ee R? >. (3.12) 
0 1 


T! = (3.13) 


R™ —R't 
or 1 


最 后 ， 为 了 保持 符号 的 简洁 ， 在 不 引起 歧义 的 情况 下 ， 我 们 以 后 不 
区 别 齐 次 坐标 与 普通 坐标 的 符号 ， 默 认 使 用 的 是 符合 运算 法 则 的 那 一 种 
。 例 如 ， 当 我 们 写 Ta 时 ， 使 用 的 是 齐 次 坐标 (不 然 没 法 计算 ) 。 而 写 Ra 
时 ， 使 用 的 是 非 齐 次 坐标 。 如 果 写 在 一 个 等 式 中 ， 就 假设 齐 次 坐标 到 普 
通 坐 标的 转换 ， 是 已 经 做 好 了 的 一 一 因为 齐 次 坐标 和 非 齐 次 坐标 之 间 的 
转换 事实 上 非常 容易 。 

回顾 一 下 : 首先 ， 介 绍 了 向 量 及 其 坐标 表示 ， 并 介绍 了 向 量 间 的 运 
算 ; 然后， 坐标 系 之 间 的 运动 由 欧 氏 变换 描述 ， 它 由 平移 和 旋转 组 成 。 
旋转 可 以 由 旋转 矩阵 5O(3) 描 述 ， 而 平移 直接 由 一 个 R 向 量 描述 。 最 后 ， 
如 果 将 平移 和 旋转 放 在 一 个 矩阵 中 ， 就 形成 了 变换 矩阵 SE(3)。 


3.2 ”实践 : Eigen 


本 讲 的 实践 部 分 有 两 节 。 第 一 部 分 中 ， 将 讲解 如 何 使 用 Eigen 来 表示 
和 矩阵、 向 量 ， 随 后 引申 至 旋转 矩阵 与 变换 矩阵 的 计算 。 本 节 的 代码 在 
slambook/ch3/useEigen 中 。 


Eigen” 是 一 个 C++ 开源 线性 代数 库 。 它 提供 了 快速 的 有 关 和 窍 阵 的 线 
性 代数 运算 ， 还 包括 解 方程 等 功能 。 许 多 上 层 的 软件 库 也 使 用 Eigen 进 行 
和 矩阵 运算 ， 包 括 g20、Sophus 等 。 照 应 本 讲 的 理论 部 分 ， 我 们 来 学 习 一 下 
Eigen 的 编程 。 


你 的 PC 上 可 能 还 没有 安装 Eigen。 请 输入 以 下 命令 进行 安装 : 


1 | sudo apt-get install libeigen3-dev 


大 部 分 常用 的 库 都 已 在 Ubuntu 软 件 源 中 提供 。 以 后 ， 若 想 要 安装 某 
个 库 ， 不 妨 先 搜索 一 下 Ubuntu 的 软件 源 中 是 否 已 提供 。 通 过 apt 命 令 ， 我 
们 能 够 方便 地 安装 Eigen。 回 顾 上 一 讲 的 知识 ， 我 们 知道 一 个 库 由 头 文件 
和 库 文件 组 成 。Eigen 头 文件 的 默认 位 置 在 “/usr/include/eigen3/” 中 。 如 果 
不 确定 ， 可 以 输入 以 下 命令 查找 : 


1 | sudo updatedb 
2 | locate eigen3 


相 比 于 其 他 库 ，Eigen 的 特殊 之 处 在 于 ， 它 是 一 个 纯 用 头 文件 搭建 起 
来 的 库 (这 非常 神奇 ! ) 。 这 意味 着 你 只 能 找到 它 的 头 文件 ， 而 没有 .so 
或 .a 那样 的 二 进 制 文件 。 在 使 用 时 ， 只 需 引 入 Eigen 的 头 文件 即 可 ， 不 需 
要 链接 库 文件 《因为 它 没有 库 文件 ) 。 下 面 写 一 段 代 码 来 实际 练习 一 下 
Eigen 的 使 用 : 


由 slambook/ch3/useEigen/eigenMatrix.cpp 


#include <iostream> 


#include <ctime> 


using namespace std; 


// Eigen 部 分 
#include <Eigen/Core> 
// 稠密 矩阵 的 代数 运算 ( 逆 、 特 征 值 等 ) 


#include <Eigen/Dense> 
#define MATRIX_SIZE 50 


FORO 
* 本 程序 演示 了 Eigen 基本 类 型 的 使 用 


IRI ARK AR AAI IR AR I /- 


int main( int argc, char** argv ) 
{ 
// Eigen VASE ARARE PA, CE-AR, CHMARFHA: 数据 类 型 ， 行 ， 
列 
// 声明 一 个 2x3 的 float 矩阵 
Eigen::Matrix<float, 2, 3> matrix_23; 
// 同时 ，Eigen 通过 typedef 提供 了 许多 内 置 类 型 ， 不 过 底层 仍 是 Eigen: :Matriz 
// 例如 Vector3d 实质 上 是 Eigen::Matriz<double, 3, 1> 
Eigen: :Vector3d v_3d; 
// 还 有 Matriz3d 实质 上 是 Eigen::Matriz<double, 3, 3> 
Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); // 初 始 化 为 零 
// 如 果 不 确定 矩阵 大 小 ， 可 以 使 用 动态 大 小 的 天 阵 
Eigen: :Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic; 
// 更 简单 的 
Eigen: :MatrixXd matrix_x; 


// 这 种 类 型 还 有 很 多 ， 我 们 不 一 一 列举 


// F i AT AE E hh ARAE 

// 输入 数据 

matrix 23 << 1, 2, 3,4, 5, 6: 
// 输出 


cout << matrix_23 << endl; 


// 用 (0) 访问 矩阵 中 的 元 素 
for (int i=0; i<1; i++) 
for (int j=0; j<2; j++) 


cout<<matrix_23(i,j)<<endl; 


Wisdi << SB, 2p Ts 


// EE Fo m HOF (HK PE EMG He 4B Fo HEE ) 


// 但 是 在 这 里 你 不 能 混合 两 种 不 同类 型 的 矩阵 ， 像 这 样 是 错 的 


// Eigen::Matrir<double, 2, 1> result_wrong_type = matriz_23 * v_3d; 


// 应 该 显 式 转换 


Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d; 


cout << result << endl; 


// WAFER AB OE 4% BE EE 89 EE 

// 试 着 取消 下 面 的 注释 ， 看 看 会 报 什 么 错 
// Eigen: :Matriz<double, 2, 3> result_wrong_dimension 
>() * vy_3d; 


// — % KE REF 
// 四 则 运算 就 不 演示 了 ， 直 接 用 对 应 的 运算 符 即 可 。 


matrix_33 = Eigen::Matrix3d::Random() ; 


cout << matrix_33 << endl << endl; 


cout << matrix_33.transpose() << endl; 
cout << matrix_33.sum() << endl; 

cout << matrix_33.trace() << endl; 

cout << 10*matrix_33 << endl; 

cout << matrix_33.inverse() << endl; 
cout << matrix_33.determinant() << endl; 
// 特征 值 


// 实 对 称 和 矩阵 可 以 保证 对 角 化 成 功 


Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( matrix_33. 


transpose() * matrix_33 ); 


cout << "Eigen values = " << eigen_solver.eigenvalues() << endl; 
cout << "Eigen vectors = " << eigen_solver.eigenvectors() << endl; 
// 解 方程 


// 我 们 求解 matriz NN * zm = v_Na 这 个 方程 


// 转 置 
// 各 元 素 和 
// 迹 
Ves 
4/1% 

// 行 列 式 


// N 的 大 小 在 前 边 的 宏 里 定义 ， 抵 阵 由 随机 数 生成 


// 直接 求 北 自 然 是 最 直接 的 ， 但 是 求 北 运算 量 大 


Eigen::Matrix< double, MATRIX_SIZE, MATRIX_SIZE > matrix_NN; 
matrix_NN = Eigen: :MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE ); 
Eigen: :Matrix< double, MATRIX_SIZE, 


1> v_Nd; 


v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE,1 ); 


clock_t time_stt = clock(); // 计时 


matriz_23.cast<double 


85 // BARE 

86 Eigen: :Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd; 

87 cout <<"time use in normal invers is " << 1000* (clock() - time_stt)/(double) 
CLOCKS_PER_SEC << "ms"<< endl; 


89 // 通常 用 矩阵 分 解 来 求 ， 例 如 QR 分解， 速度 会 快 很 多 

90 time_stt = clock(); 

91 x = matrix_NN.colPivHouseholderQr().solve(v_Nd) ; 

92 cout <<"time use in Qr compsition is " <<1000* (clock() - time_stt)/(double) 


CLOCKS_PER_SEC <<"ms" << endl; 


94 return 0; 


这 个 例 程 演示 了 Eigen 和 矩阵 的 基本 操作 与 运算 。 要 编译 它 ， 需 要 在 
CMakeLists.txt 里 指定 Eigen 的 头 文件 目录 : 


1 |# 添加 头 文 件 


2 | include_directories( "/usr/include/eigen3" ) 


重复 一 遍 ， 因 为 Eigen 库 只 有 头 文 件 ， 所 以 不 需要 再 用 
target_link_libraries 语 句 将 程序 链接 到 库 上 。 不 过 ， 对 于 其 他 大 部 分 库 ， 
多 数 时 候 需 要 用 到 链接 命令 。 这 里 的 做 法 并 不 见得 是 最 好 的 ， 因 为 其 他 
人 可 能 把 Eigen 安 装 在 了 不 同位 置 ， 那 么 就 必须 手动 修改 这 里 的 头 文件 目 
录 。 在 之 后 的 工作 中 ， 我 们 会 使 用 fi nd_package 命 令 去 搜索 库 ， 不 过 在 
本 讲 中 暂时 保持 这 个 样子 。 编 译 好 这 个 程序 后 ， 运 行 它 ， 可 以 看 到 各 和 矩 
阵 的 输出 结果 。 


1 |% build/eigenMatrix 

2 |123 

3 |456 

4 |1 

5 |2 

6 |10 

7 |28 

8 | 0.680375 0.59688 -0.329554 
9 | -0.211234 0.823295 0.536459 
0.566198 -0.604897 -0.444451 


© 


由 于 在 代码 中 给 出 了 详细 的 注释 ， 在 此 就 不 一 一 解释 每 行 语句 了 。 
本 书 中 ， 我 们 将 仅 给 出 几 处 重要 地 方 的 说 明 (后 面 的 实践 部 分 亦 将 保持 
这 个 风格 ) o 


1. 读 者 最 好 亲手 输入 一 遍 上 面 的 代码 (不 包括 注释 ) 。 至 少 要 编译 
行 一 遍 上 面 的 程序 。 

2.Kdevelop 可 能 不 会 提示 C++ 成 员 运 算 ， 这 是 它 做 得 不 够 完善 导致 
的 。 请 照 着 上 面 的 内 容 输入 即 可 ， 不 必 理 会 它 是 否 提示 错误 。 


3.Eigen 提 供 的 矩阵 和 MATLAB 很 相似 ， 几 乎 所 有 的 数据 都 当 作 和 矩阵 
来 处 理 。 但 是 ， 为 了 实现 更 好 的 效率 ， 在 Eigen 中 需要 指定 和 矩 阵 的 大 小 和 
类 型 。 对 于 在 编译 时 期 就 知道 大 小 的 和 矩阵， 处 理 起 来 会 比 动态 变化 大 小 
的 和 矩阵 更 快 一 些 。 因 此 ， 像 旋转 和 矩 阵 、 变 换 和 矩阵 这 样 的 数据 ， 完 全 可 在 
编译 时 期 确定 它们 的 大 小 和 数据 类 型 。 


4.Eigen 内 部 的 矩阵 实现 比较 复杂 ， 这 里 不 做 介绍 ， 我 们 希望 你 像 使 
用 float、double 等 内 置 数据 类 型 那样 使 用 Eigen 的 矩阵 。 这 应 该 是 符合 其 
设计 初衷 的 。 

5.Eigen 和 矩 阵 不 支持 自动 类 型 提升 ， 这 和 C++ 的 内 建 数 据 类 型 有 较 大 差 
异 。 在 C++ 程序 中 ， 我 们 可 以 把 一 个 float 数据 和 double 数 据 相 加 、 相 乘 ， 
编译 器 会 自动 把 数据 类 型 转换 为 最 合适 的 那 种 。 而 在 Eigen 中 ， 出 于 性 能 
的 考虑 ， 必 须 显 式 地 对 和 矩阵 类 型 进行 转换 。 而 如 果 忘 了 这 样 做 ，Eigen 会 
(不 太 友 好 地 ) 提示 你 一 个 “YOU MIXED DIFFERENT NUMERIC 
TYPES...” 的 编译 错误 。 你 可 以 尝试 找 一 下 这 条 信息 出 现在 错误 提示 的 哪 
个 部 分 。 如 果 错 误 信 息 太 长 最 好 保存 到 一 个 文件 里 再 找 。 


Bl 


6. 同 理 ， 在 计算 过 程 中 也 需要 保证 矩阵 维 数 的 正确 性 ， 否 则 会 出 现 
“YOU MIXED MATRICES OF DIFFERENT SIZES” 错 误 。 请 不 要 抱怨 这 种 
错误 提示 方式 ， 对 于 C++ 模板 元 编程 ， 能 够 提示 出 可 以 阅读 的 信息 已 经 是 
很 平 运 的 了 。 以 后 ， 若 发 现 Eigen 出 错 ， 你 可 以 直接 寻找 大 写 的 部 分 ， 推 
测 出 了 什么 问题 。 


7. 我 们 的 例 程 只 介绍 了 基本 的 矩阵 运算 。 你 可 以 阅读 

http://eigen.tuxfamily.org/dox-devel/modules.html 学 习 更 多 的 Eigen 知 识 。 这 

只 演示 了 最 简单 的 部 分 ， 能 看 民 演 示 程 序 不 等 于 你 已 经 能 够 熟练 操作 
Eigeno 


最 后 一 段 代 码 中 比较 了 求 逆 与 求 QR 分 解 的 运行 效率 ， 你 可 以 看 看 目 
己 机 器 上 的 时 间 差 异 ， 两 种 方法 是 否 有 了 明显 的 差异 ? 


3.3 ”旋转 向 量 和 欧 拉 角 


3.3.1 ”旋转 向 量 


我 们 重新 回 到 理论 部 分 。 有 了 旋转 矩阵 来 描述 旋转 ， 有 了 变换 矩阵 
描述 一 个 6 自由 度 的 三 维 刚体 运动 ， 是 不 是 已 经 足够 了 呢 ? 和 矩阵 表示 方式 
至 少 有 以 下 几 个 缺 后 : 


1.SO(3) 的 旋转 矩阵 有 9 个 量 ， 但 一 次 旋转 只 有 3 个 自由 度 。 因 此 这 种 
表达 方式 是 见 余 的 。 同 理 ， 变 换 和 矩阵 用 16 个 量 表达 了 6 自由 度 的 变换 。 那 
么 ， 是 否 有 更 紧凑 的 表示 呢 ? 


2. 旋 转 矩 阵 上 自身 带 有 约束 : 它 必 须 是 个 正 交 和 矩阵 ， 且 行列 式 为 1。 变 
换 和 矩 阵 也 是 如 此 。 当 想 要 估计 或 优化 一 个 旋转 矩阵 /变换 矩 阵 时 ， 这 些 约 
束 会 使 得 求解 变 得 更 困难 。 

因此 ， 我 们 希望 有 一 种 方式 能 够 紧凑 地 描述 旋转 和 平移 。 例 如 ， 用 
一 个 三 维 向 量 表达 旋转 ， 用 六 维 向 量 表达 变换 ， 可 行 吗 ? 事实 上 ， 在 前 
面 介绍 外 积 的 那 部 分 ， 我 们 提 到 过 如 何 做 这 件 事 。 我 们 介绍 了 如 何 用 外 
积 表达 两 个 向 量 的 旋转 关系 。 对 于 坐标 系 的 旋转 ， 我 们 知道 ， 任 意 旋转 
都 可 以 用 一 个 旋转 轴 和 一 个 旋转 角 来 刻画 。 于 是 ， 我 们 可 以 使 用 一 个 向 
量 ， 其 方向 与 旋转 轴 一 致 ， 而 长 度 等 于 旋转 角 。 这 种 向 量 称 为 旋转 向 量 
(或 轴 角 ，Axis-Angle) 。 这 种 表示 法 只 需 一 个 三 维 向 量 即 可 描述 旋转 。 


同样 ， 对 于 变换 矩阵 ， 我 们 使 用 一 个 旋转 向 量 和 一 个 平移 向 量 即 可 表达 
一 次 变换 。 这 时 的 维 数 正好 是 六 维 。 

事实 上 ， 旋 转向 量 就 是 下 一 讲 准 备 介绍 的 李 代 数 。 所 以 本 讲 中 读者 
只 需 知 道 旋转 可 以 这 样 表 示 即 可 。 剩 下 的 问题 是 ， 旋 转向 量 和 旋转 和 矩阵 
之 间 是 如 何 转 换 的 呢 ? 假设 有 一 个 旋转 轴 为 + ， 角 度 为 9 的 旋转 ， 显 然 ， 
它 对 应 的 旋转 向 量 为 bn 。 从 旋转 向 量 到 旋转 矩阵 的 转换 过 程 由 罗 德 里 格 
斯 公式 (Rodrigues’s Formula) 表明 ， 由 于 推导 过 程 比较 复杂 ， 这 里 不 作 
描述 ， 只 给 出 转换 的 结果 3 : 


R=cos@I + (1—cos0)nn' + sin fn^. (3.14) 


符号 ^ 是 向 量 到 反对 称 的 转换 符 ， 见 式 (3.3) o RZ, RIEA 
计算 从 一 个 旋转 矩阵 到 旋转 向 量 的 转换 。 对 于 转角 9 ， 有 : 


tr(R) = cosôðtr(I)+ (1 -— cos0)tr (nn*) + sin 6tr(n’) 
= 3cos0 二 (1 一 cos0) (3.15) 
= 1+2cosé. 

因此 : 
0 = arccos( "9 — 1), (3.16) 


关于 转轴 n ， 由 于 旋转 轴 上 的 向 量 在 旋转 后 不 发 生 改变 ， 说 明 
Rn =n. 


因此 ， 转 轴 n <2 7B PER 特征 值 1 对 应 的 特征 向 量 。 求 解 此 方程 ， 再 归 
一 化 ， 就 得 到 了 旋转 轴 。 读 者 也 可 以 从 * 旋 转轴 经 过 旋转 之 后 不 变 ” 的 几 
何 角度 看 待 这 个 方程 。 顺 便 提 一 下 ， 这 里 的 两 个 转换 公式 在 下 一 讲 仍 将 
出 现 ， 你 会 发 现 它们 正 是 SO(3) 上 李 群 与 李 代 数 的 对 应 关系 。 


3.3.2 ” 欧 拉 角 


下 面 来 说 说 欧 拉 角 。 


无 论 是 旋转 矩阵、 旋转 向 量 ， 它 们 虽然 能 描述 旋转 ， 但 对 我 们 人 类 
是 非常 不 直观 的 。 当 我 们 看 到 一 个 旋转 矩阵 或 旋转 向 量 时 ， 很 难 想 象 出 
这 个 旋转 究竟 是 什么 样 的 。 当 它们 变换 时 ， 我 们 也 不 知道 物体 是 向 哪个 
方向 在 转动 。 而 欧 拉 角 则 提供 了 一 种 非常 直观 的 方式 来 描述 旋转 它 
使 用 了 3 个 分 离 的 转角 ， 把 一 个 旋转 分 解 成 3 次 绕 不 同 轴 的 旋转 。 当 然 ， 
由 于 分 解 方式 有 许多 种 ， 所 以 欧 拉 角 也 存在 着 不 同 的 定义 方法 。 比 如 
说 ， 先 绕 X 轴 旋 转 ， 再 绕 Y 轴 ， 最 后 绕 Z 轴 ， 就 得 到 了 一 个 XY Z 轴 的 旋 
转 。 同 理 ， 可 以 定义 ZYZ、ZYX 等 旋转 方式 。 如 果 讨 论 得 更 细 一 些 ， 还 
需要 区 分 每 次 是 绕 固定 轴 旋转 的 ， 还 是 绕 旋 转 之 后 的 轴 旋转 的 ， 这 也 会 
给 出 不 一 样 的 定义 方式 。 


你 或 许 在 航空 、 航 模 中 听 说 过 “俯仰 角 ” 偏 航 角 ”这 些 词 。 欧 拉 角 当中 
比较 常用 的 一 种 ， 便 是 用 * 偏 航 - 俯 仰 - 滚 转 ”(〈yaw-pitch-roll) 3 个 角度 来 描 
述 一 个 旋转 的 。 由 于 它 等 价 于 ZYX 轴 的 旋转 ， 因 此 就 以 ZYX 为 例 。 假 设 
一 个 刚体 的 前 方 (朝向 我 们 的 方向 ) 为 X 轴 ， 右 侧 为 Y7 轴 ， 上 方 为 Z 轴 ， 
如 图 3-3 所 示 。 那 么 ，ZYX 转角 相当 于 把 任意 旋转 分 解 成 以 下 3 个 轴 上 的 
转角 : 


1. 绕 物体 的 Z 轴 旋 转 ， 得 到 偏 航 角 yaw; 
2. 绕 旋转 之 后 的 Y 轴 旋 转 ， 得 到 俯仰 角 pitch ; 
3. 绕 旋转 之 后 的 X 轴 旋转 ， 得 到 滚 转角 roll。 


此 时 ， 可 以 使 用 [rp,y 这 样 一 个 三 维 的 向 量 描述 任意 旋转 。 这 个 向 
量 十 分 直观 ， 我 们 可 以 从 这 个 向 量 想象 出 旋转 的 过 程 。 其 他 的 欧 拉 角 亦 
是 通过 这 种 方式 ， 把 旋转 分 解 到 3 个 轴 上 ， 得 到 一 个 三 维 的 向 量 ， 只 不 过 
选用 的 轴 及 顺序 不 一 样 。 这 里 介绍 的 rpy 角 是 比较 单 用 的 一 种 ， 只 有 很 少 
的 欧 拉 角 种 类 会 有 rpy 这 样 脸 炙 人 口 的 名 字 。 不 同 的 欧 拉 角 是 按照 旋转 轴 
的 顺序 来 称呼 的 。 例 如 ，rpy 角 的 旋转 顺序 是 ZYX 。 同 样 ， 也 有 XY Z,ZYZ 
这 样 的 欧 拉 角 一 一 但 是 它们 就 没有 专门 的 名 字 了 。 值 得 一 提 的 是 ， 大 部 
分 领域 在 使 用 欧 拉 角 时 都 有 各 自 的 坐标 方向 和 顺序 上 的 习惯 ， 不 一 定 和 
我 们 这 里 说 的 相同 。 


欧 拉 角 的 一 个 重大 缺点 是 会 碰 到 著名 的 万 向 锁 问 题 (Gimbal Lock"! 
) : 在 俯仰 角 为 + 90 时， 第 一 次 旋转 与 第 三 次 旋转 将 使 用 同一 个 轴 ， 使 
得 系统 丢失 了 一 个 自由 度 (由 3 次 旋转 变 成 了 2 次 旋转 ) 。 这 被 称 为 奇异 
性 间 题 ， 在 其 他 形式 的 欧 拉 角 中 也 同样 存在 。 理 论 上 可 以 证 明 ， 只 要 想 
用 3 个 实效 来 表达 三 维 旋转 时 ， 都 会 不 可 避免 地 碰 到 奇异 性 问题 。 由 于 这 


种 原理 ， 欧 拉 角 不 适 于 插值 和 和 迭代， 往往 只 用 于 人 机 交互 中 。 我 们 也 很 
少 在 SLAM 程 序 中 直接 使 用 欧 拉 角 表达 姿态 ， 同 样 不 会 在 滤波 或 优化 中 使 
用 欧 拉 角 表达 旋转 〈 因 为 它 具 有 奇异 性 ) 。 不 过 ， 若 你 想 验 证 自己 的 算 
法 是 否 有 错 ， 转 换 成 欧 拉 角 能 够 快速 分 辨 结果 是 否 正 确 。 
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图 3-3” 欧 拉 角 的 旋转 示意 图 。 上 方 为 ZYX 角 定义 。 下 方 为 pitch=90“ 时， 第 三 次 旋转 与 第 一 
次 滚 转 角 相同 ， 使 得 系统 丢失 了 一 个 自由 度 。 如 果 你 还 没有 理解 万 向 锁 ， 可 以 看 看 相关 视 
频 ， 理 解 起 来 会 更 方便 。 


3.4 ”四 元 数 


3.4.1 ”四 元 数 的 定义 


旋转 矩阵 用 9 个 量 描述 3 自由 度 的 旋转 ， 具 有 允 余 性 ， 欧 拉 角 和 旋转 
向 量 是 紧凑 的 ， 但 具有 奇异 性 。 事 实 上 ， 我 们 找 不 到 不 带 奇 异性 的 三 维 
向 量 描述 方式 "9 。 这 有 点 类 似 于 用 两 个 坐标 表示 地 球 表面 (如 经 度 和 纬 
È) ， 将 必定 存在 奇异 性 (纬度 为 + 90“ 时 经 度 无 意义 ) 。 三 维 旋转 是 一 
个 三 维 流 形 ， 想 要 无 奇异 性 地 表达 它 ， 用 3 个 量 是 不 够 的 。 

回忆 以 前 学 习 过 的 复数 。 我 们 用 复数 集 C 表 示 复 平面 上 的 向 量 ， 而 复 
数 的 乘法 则 表示 复 平面 上 的 旋转 : 例如 ， 乘 上 复数 i 相当 于 逆 时 针 把 一 个 


复 向 量 旋转 90 " 。 类 似 地 ， 在 表达 三 维 空间 旋转 时 ， 也 有 一 种 类 似 于 复数 
的 代数 : 四 元 数 (Quaternion) 。 四 元 数 是 Hamilton 找 到 的 一 种 扩展 的 复 
数 。 它 既是 紧凑 的 ， 也 没有 奇异 性 。 如 果 说 缺点 ， 四 元 数 不 够 直观 ， 其 
运算 稍 复杂 些 。 

一 个 四 元 数 q 拥有 一 个 实 部 和 三 个 虚 部 。 本 书 把 实 部 写 在 前 面 (也 有 
地 方 把 实 部 写 在 后 面 ) ， 像 下 面 这 样 : 


q = qo + qii + Gj + q3k, (3.17) 


其 中 ij,k 为 四 元 数 的 三 个 虚 部 。 这 三 个 虚 部 满足 以 下 关系 式 : 


i? = j? = k? = —1 

ij = k, ji = —k 

A (3.18) 
jk=i,ki= 一; 


ki = j,ik = —j 


由 于 它 的 这 种 特殊 表示 形式 ， 有 时 人 们 也 用 一 个 标量 和 一 个 向 量 来 
表达 四 元 数 : 


q=[s,v], s= ER, v= [q,q2,q3]" € R?, 


这 里 ，s 称 为 四 元 数 的 实 部 ， 而 v 称 为 它 的 虚 部 。 如 果 一 个 四 元 数 的 
虚 部 为 0， 称 之 为 实 四 元 数 。 反 之 ， 若 它 的 实 部 为 0， 则 称 之 为 虚 四 元 数 

这 和 复数 非常 相似 。 考 虑 到 三 维 空间 需要 3 个 轴 ， 四 元 数 也 有 3 个 虚 
部 ， 那 么 ， 一 个 虚 四 元 数 能 不 能 对 应 到 一 个 空间 点 呢 ? 事实 上 我 们 就 是 
这 样 做 的 。 同 理 ， 我 们 知道 一 个 模 长 为 1 的 复数 可 以 表示 复 平 面 上 的 纯 旋 
转 (没有 长 度 的 缩放 ) ， 那 么 ， 三 维 空间 中 的 旋转 是 否 能 用 单位 四 元 数 
表达 呢 ? 答案 也 是 肯定 的 。 

我 们 能 用 单位 四 元 数 表示 三 维 空间 中 任意 一 个 旋转 ， 不 过 这 种 表达 
方式 和 复数 有 着 微妙 的 不 同 。 在 复数 中 ， 乘 以 ;意味 着 旋转 90 " 。 这 是 否 
意味 着 四 元 数 中 ， 乘 i 就 是 绕 i 轴 旋 转 90“? AA, =k 是 否 意味 着 ， 先 
绕 i 转 90" ， 再 绕 ) 转 90 " ， 就 等 于 绕 k 转 - 90°? 读者 可 以 找 一 部 手机 比划 


一 下 一 一 然后 你 会 发 现 情况 并 不 是 这 样 。 正 确 的 情形 应 该 是 ， 乘 以 对 应 
着 旋转 180" ， 这 样 才能 保证 i =-k 的 性 质 。 而 i?” =- 1， 意 味 着 绕 i 轴 旋 转 
360 “后 得 到 一 个 相反 的 东西 。 这 个 东西 要 旋转 两 周 才 会 和 它 原 先 的 样子 
相等 。 

这 似乎 有 些 辫 妙 了 ， 完 整 的 解释 需要 引入 太 多 额外 的 东西 ， 我 们 还 
是 冷静 一 下 回 到 眼前 。 至 少 ， 我 们 知道 单位 四 元 数 能 够 表达 三 维 空间 的 
旋转 。 这 种 表达 方式 和 旋转 矩阵 、 旋 转向 量 有 什么 关系 呢 ? 我 们 不 妨 先 
来 看 旋转 向 量 。 假 设 某 个 旋转 是 绕 单 位 向 量 n =n, ,n, ,n, J] 进行 了 角度 为 6 
的 旋转 ， 那 么 这 个 旋转 的 四 元 数 形式 为 


a 2.8 2.6. 4.6)" 
q= COS 5, nz Sin 3, Ny sin 5, nz sin 5 : (3.19) 


反之 ， 亦 可 从 单位 四 元 数 中 计算 出 对 应 旋转 轴 与 夹 角 : 


r = 2 arccos qo (3.20) 


"=| 


[nz, Ny, nz] qı, q2, q3] /sin £ 


这 个 式 子 给 了 我 们 一 种 微妙 的 “ 转 了 一 半 ” 的 感觉 。 同 样 ， 对 式 
(3.19) 的 9 加 上 2r ， 我 们 得 到 一 个 相同 的 旋转 ， 但 此 时 对 应 的 四 元 数 变 
成 了 -q 。 因 此 ， 在 四 元 数 中 ， 任 意 的 旋转 都 可 以 由 两 个 互 为 相反 数 的 四 
元 数 表示 。 同 理 ， 取 9 为 0， 则 得 到 一 个 没有 任何 旋转 的 实 四 元 数 : 


qo = [£1,0,0,0]". (3.21) 
3.4.2 ”四 元 数 的 运算 
四 元 数 和 通常 复数 一 样 ， 可 以 进行 一 系列 的 运算 。 常 见 的 有 四 则 运 
F AR RKE, HHE FANNA. 
现 有 两 个 四 元 数 q, ,q, ， 它 们 的 向 量 表示 为 [s, ,v ], [s, ,w ]， 或 者 原始 
四 元 数 表示 为 


da = Sa + Lat + Yaj T Zak, qb = Sb + £pi + Yoj + Zpk. 


那么 ， 其 运算 可 表示 如 下 。 
1. 加 法 和 减法 
四 元 数 q, ,q, 的 加 减 运 算 为 
Gat qb = [Sa = Sb, Va E vo]. (3.22) 


2. 乘 法 
乘法 是 把 qd, 的 每 一 项 与 q 的 每 项 相 乘 ， 最 后 相 加 ， 虚 部 要 按照 式 
(3.18) 进行 。 整 理 可 得 : 
dadb = SaSb — Lab — YaYb — ZaZb 

十 (sa2Z5 + Taso + Yar — Zayb) i (3.23) 
+ (SaYb — Lar + YaSb + Zits) J 
+ (SaZp + LaYb — Yao + ZaSp) k. 

虽然 稍为 复杂 ， 但 形式 上 是 整齐 有 序 的 。 如 果 写 成 向 量 形式 并 利用 

内 外 积 运算 ， 该 表达 会 更 加 简洁 : 


dadb = [sasp Fi UTUb， SaUb 十 SbVa 十 Va X vp | 。 (3.24) 


在 该 乘法 定义 下 ， 两 个 实 的 四 元 数 乘积 仍 是 实 的 ， 这 与 复数 也 是 一 
致 的 。 然 而 ， 注 意 到 ， 由 于 最 后 一 项 外 积 的 存在 ， 四 元 数 乘法 通常 是 不 
可 交换 的 ， 除 非 v 和 v, 在 Rs 中 共 线 ， 此 时 外 积 项 为 零 。 

3.446 

PORK AH oe SE ESPANA AB RK: 


Gd = Sa — Lal — Yaj — Zak = [Sa, —Val- (3.25) 


MaRS AASR, 2AA, BRBARKN 
FA: 


q*q = qq* =([s, + v'v,0). (3.26) 


4. 模 长 


四 元 数 的 模 长 定义 为 
llgall = Visa + za + ya + za- (3.27) 


可 以 验证 ， 两 个 四 元 数 乘 积 的 模 即 为 模 的 乘积 。 这 保证 了 单位 四 元 
效 相 乘 后 仍 是 单位 四 元 效 。 


llqagl| = llgallllgoll. (3.28) 
5. 逆 
一 个 四 元 数 的 逆 为 

q "= q*/llall”. (3.29) 
按 此 定义 ， 四 元 数 和 自己 的 逆 的 乘积 为 实 四 元 数 1: 

qq "=q gq=1. (3.30) 


如 果 gq 为 单位 四 元 数 ， 其 逆 和 共 罗 就 是 同一 个 量 。 同 时 ， 乘 积 的 逆 有 
和 矩阵 相似 的 性 质 : 


(qaq) = qa". (3.31) 
6. 数 乘 与 点 乘 
和 向 量 相似 ， 四 元 数 可 以 与 数 相 乘 : 
kq = [ks, kv]. (3.32) 
点 乘 是 指 两 个 四 元 数 每 个 位 置 上 的 数值 分 别 相 乘 : 
da .gp = sasb + LaXvi + Yayos + Za2k- (3.33) 


3.4.3 ”用 四 元 数 表示 旋转 


我 们 可 以 用 四 元 数 表达 对 一 个 点 的 旋转 。 假 设 一 个 空间 三 维 点 p = 
[xyz]E Rs ， 以 及 一 个 由 轴 角 n,6 指定 的 旋转 。 三 维 点 p 经 过 旋转 之 后 变 


Alp 。 如 果 使 用 和 矩阵 描述 ， 那 么 有 p' =Rp 。 而 如 果 用 四 元 数 描述 旋转 ， 它 
们 的 关系 又 如 何 来 表达 呢 ? 


首先 ， 把 三 维 空间 点 用 一 个 虚 四 元 数 来 描述 : 
p= [0, x,y, z] = [0, v]. 
这 相当 于 把 四 元 数 的 3 个 虚 部 与 空间 中 的 3 个 轴 相 对 应 。 然 后 ， 参 照 
式 (3.19)， 用 四 元 数 q 表示 这 个 旋转 : 


= cas n sin 0 
a laces 


那么 ， 旋 转 后 的 点 p' 即 可 表示 为 这 样 的 乘积 : 


p' = qpq +. (3.34) 


可 以 验证 《 留 作 习 题 ) ， 计 算 结 果 的 实 部 为 0， 故 为 纯 虚 四 元 数 。 其 
虚 部 的 3 个 分 量 表示 旗 转 后 3D 点 的 坐标 。 


3.4.4 ”四 元 数 到 旋转 和 矩阵 的 转换 


任意 单位 四 元 数 摘 述 了 一 个 旋转 ， 该 旋转 亦 可 用 旋转 矩阵 或 旋转 向 
量 描述 。 从 旋转 向 量 到 四 元 数 的 转换 方式 已 在 式 (3.20) 中 给 出 。 因 此 ， 
现在 看 来 把 四 元 数 转 换 为 矩阵 的 最 直观 方法 ， 是 先 把 四 元 数 q 转换 为 轴 角 
9 和 n ， 然 后 再 根据 罗 德 里 格 斯 公式 转换 为 和 矩阵。 不 过 那样 要 计算 一 个 
arccos 冰 数 ， 代 价 较 大 。 实 际 上 这 个 计算 是 可 以 通过 一 定 的 技巧 绕 过 的 。 
这 里 省 略 推导 过 程 ， 直 接 给 出 四 元 数 到 旋转 和 矩阵 的 转换 方式 。 


设 四 元 数 q =q ,+9q | i +9q ,j +q sk ， 对 应 的 旋转 矩阵 R 为 


1 — 2q2 — 2q 2qıq2 +2qoq3 2¢193 — 2q0g2 
R= | 2qıq2 — 2qoq3 1—24? — 243 2q2q3 十 24091 (3.35) 


2q1q3 + 2qoq2 2q2q3 — 2qoq1 1— 2q? — 242. 


RZ, bets FBO TARAS FRM BRIERE AR ={m, hijE 
[1, 2, 3]， 其 对 应 的 四 元 数 q 由 下 式 给 出 : 


tr(R) 十 1 M23 — M32 731 — M13 M12 — M21 
41 = naa m  -: 
2 4qo 4qo 4qo 


值得 一 提 的 是 ， 由 于 q 和 -9 表示 同一 个 旋转 ， 事 实 上 一 个 R 对 应 的 四 
元 数 表示 并 不 是 唯一 的 。 同 时 ， 除 了 上 面 给 出 的 转换 方式 之 外 ， 还 存在 
其 他 几 种 计算 方法 ， 而 本 书 都 省 略 了 。 实 际 编程 中 ， 当 qd , 接近 0 时 ， 其 余 
3 个 分 量 会 非常 大 ， 导 致 解 不 稳定 ， 此 时 我 们 再 考虑 使 用 其 他 的 方式 进行 
转换 。 

最 后 ， 无 论 是 四 元 数 、 旋 转 矩 阵 还 是 轴 角 ， 它 们 都 可 以 用 来 描述 同 
一 个 旋转 。 我 们 应 该 在 实际 中 选择 最 为 方便 的 形式 ， 而 不 必 拘 泥 于 某 种 
特定 的 形式 。 在 随后 的 实践 和 习题 中 ， 我 们 会 演示 各 种 表达 方式 之 间 的 
转换 ， 以 加 深 读者 的 印象 。 


3.5 * 相 似 、 仿 射 、 射 影 变 换 


3D 空 间 中 的 变换 ， 除 了 欧 氏 变换 之 外 ， 还 存在 其 他 几 种 ， 只 不 过 欧 
氏 变 换 是 最 简单 的 。 它 们 一 部 分 和 测量 几何 有 关 ， 因 为 在 之 后 的 讲解 中 
可 能 会 提 到 ， 所 以 先 罗 列 出 来 。 欧 氏 变 换 保 持 了 向 量 的 长 度 和 夹 角 ， 相 
当 于 我 们 把 一 个 刚体 原封 不 动 地 进行 了 移动 或 旋转 ， 不 改变 它 自身 的 样 
子 。 而 其 他 几 种 变换 则 会 改变 它 的 外 形 。 它 们 都 拥有 类 似 的 矩阵 表示 。 


1. 相 似 变 换 
相似 变换 比 欧 氏 变 换 多 了 一 个 自由 度 ， 它 允许 物体 进行 均匀 缩放 ， 
其 矩阵 表示 为 


(3.36) 


(3.37) 


注意 到 旋转 部 分 多 了 一 个 缩放 因子 s ， 表 示 我 们 在 对 向 量 旋转 之 后 ， 
AL xyz 三 个 坐标 上 进行 均匀 缩放 。 由 于 含有 缩放 ， 相 似 变换 不 再 保 


持 图 形 的 面积 不 变 。 你 可 以 想象 一 个 边 长 为 1 的 立方 体 通过 相似 变换 后 ， 
变 成 边 长 为 10 的 样子 (但 仍然 是 立方 体 ) 。 


2. 仿 射 变换 
仿 射 变换 的 和 矩阵 形式 如 下 : 


A t 
. (3.38) 
oT 1 | 


Ta = 


与 欧 氏 变换 不 同 的 是 ， 仿 射 变换 只 要 求 A 是 一 个 可 逆 和 矩阵 ， 而 不 必 是 
正 交 和 矩 阵 。 仿 射 变换 也 叫 正 交 投影 。 经 过 仿 射 变换 之 后 ， 立 方 体 就 不 再 
是 方 的 了 ， 但 是 各 个 面 仍然 是 平行 四 边 形 。 


3. 射 影 变换 
射影 变换 是 最 一 般 的 变换 ， 它 的 矩阵 形式 为 


(3.39) 


它 的 左上 角 为 可 逆 和 矩阵 A ， 右 上 角 为 平移 t ， 左 下 角 为 缩放 a7 。 由 于 
采用 了 齐 次 坐标 ， 当 vw =0 时 ， 我 们 可 以 对 整个 矩阵 除 以 v 得 到 一 个 右 下 角 
为 1 的 矩阵; 否则 得 到 右 下 角 为 0 的 矩阵 。 因 此 ，2D 的 射影 变换 一 共有 8 个 
自由 度 ，3D 则 共有 15 个 自由 度 。 射 影 变换 是 现在 讲 过 的 变换 中 ， 形 式 最 
为 一 般 的 。 从 真实 世界 到 相机 照片 的 变换 可 以 看 成 一 个 射影 变换 。 读 者 
可 以 想象 一 个 原本 方形 的 地 板 砖 ， 在 照片 当中 是 什么 样子 : 首先 ， 它 不 
再 是 方形 的 。 由 于 近 大 远 小 的 关系 ， 它 甚至 不 是 平行 四 边 形 ， 而 是 一 个 
不 规则 的 四 边 形 。 


表 3-1 总 结 了 目前 讲 到 的 几 种 变换 的 性 质 。 注 意 在 “不 变性 质 * 中 ， 从 
上 到 下 是 有 包含 关系 的 。 例 如 ， 欧 氏 变 换 除 了 保 体积 之 外 ， 也 具有 保平 
行 、 相 交 等 性 质 。 


表 3-1 常 见 变 换 性 质 比 较 


变换 名 称 | EER | 自由 度 不 变性 质 

欧 氏 变换 E d 6 长 度 、 夹 角 、 体 积 

相似 变 k d 7 体积 比 

afi A t Po 

仿 射 变换 F d 12 平行 性 、 体 积 比 
接触 平面 的 相交 和 相 切 


我 们 之 后 会 说 到 ， 从 真实 世界 到 相机 照片 的 变换 是 一 个 射影 变换 。 
如 果 相 机 的 焦距 为 无 穷 远 ， 那 么 这 个 变换 为 仿 射 变换 。 不 过 ， 在 详细 讲 
述 相机 模型 之 前 ， 我 们 只 要 对 它们 有 个 大 致 的 印象 即 可 。 


3.6 SRE: Eigen 几 何 模 块 


现在 ,我们 来 实际 演练 一 下 前 面 讲 到 的 各 种 旋转 表达 方式 。 我 们 将 
在 Eigen 中 使 用 四 元 数 、 欧 拉 角 和 旋转 矩阵， 演示 它们 之 间 的 变换 方式 。 
我 们 还 会 给 出 一 个 可 视 化 程序 ， 帮 助 读者 理解 这 几 个 变换 的 关系 。 


内 slambook/ch3/useGeometry/useGeometry.cpp 


#include <iostream> 


#include <cmath> 


using namespace std; 


#include <Eigen/Core> 
// Eigen 几何 模块 
#include <Eigen/Geometry> 


DOIG IR II IIE 
* 本 程序 演示 了 Eigen 几何 模块 的 使 用 方法 


SOIC IOI GIG II I kk kk tek /- 


int main( int argc, char** argv ) 


{ 


// Eigen/Geometry 模块 提供 了 各 种 旋转 和 平移 的 表示 

// 3D 旋转 矩阵 直接 使 用 Matrtiz3d 或 Matric3f 

Eigen: :Matrix3d rotation_matrix = Eigen: :Matrix3d::Identity() ; 

// 旋转 向 量 使 用 4ngle4zis， 它 底层 不 直接 是 Matric , iI T A 4HE (因为 重 载 
了 运算 符 ) 

Eigen: :AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); 
沿 Z 轴 旋 转 45 度 

cout .precision(3); 

cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl; 

用 matria() 转换 成 矩阵 

// 也 可 以 直接 赋值 

rotation_matrix = rotation_vector.toRotationMatrix(); 

// 用 AngleAcis 可 以 进行 坐标 变换 

Eigen::Vector3d v ( 1,0,0 ); 

Eigen: :Vector3d v_rotated = rotation_vector * v; 

cout<<" (1,0,0) after rotation = "<<v_rotated.transpose()<<endl; 

// 或 者 用 旋转 矩阵 

v_rotated = rotation_matrix * v; 

cout<<"(1,0,0) after rotation "<<v_rotated.transpose()<<end1; 


// 欧 拉 角 : TVA eG HEE ER RR GEA 


Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX 顺 
序 ， 即 yaw pitch roll 顺序 
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<end1l; 


// 欧 氏 变换 矩阵 使 用 Bigen::Isometry 


Eigen::Isometry3d T=Eigen::Isometry3d::Identity(); // 虽然 称 为 34 ， 实 质 上 
是 4x4 的 矩阵 
T.rotate ( rotation_vector ); // 按照 rotation_vector 进行 旋转 


T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 把 平移 向 量 设 成 (1,3,4) 


40 cout << "Transform matrix = \n" << T.matrix() <<endl; 

41 

2 // 用 变换 矩阵 进行 坐标 变换 

43 Eigen::Vector3d v_transformed = T*v; // 相当 于 Revtt 

44 cout<<"v tranformed = "<<v_transformed.transpose()<<endl; 

45 

46 // 对 于 仿 射 和 射影 变换 ， 使 用 Eigen: :Affine3d 和 Eigen::Projective3d PPT, % 

47 

48 // 四 元 数 

49 // 可 以 直接 把 hnglehzis 赋值 给 四 元 数 ， 反 之 亦 然 

50 Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector ); 

51 cout<<"quaternion = \n"<<q.coeffs() <<endl; // 注意 coeffs 的 顺序 是 (@,y,z, w) 
„w 为 实 部 ， 前 三 者 为 虚 部 

52 // 也 可 以 把 旋转 和 矩阵 赋 给 它 

53 q = Eigen::Quaterniond ( rotation_matrix ) ; 

54 cout<<"quaternion = \n"<<q.coeffs() <<endl; 

55 // 使 用 四 元 数 旋 转 一 个 向 量 ， 使 用 重 载 的 乘法 即 可 

56 v_rotated = qtv; // 注意 数学 上 是 quqt-1} 

57 cout<<" (1,0,0) after rotation = "<<v_rotated.transpose()<<endl; 

58 

59 return 0; 

oo |} 


Eigen 中 对 各 种 形式 的 表达 方式 总 结 如 下 。 请 注意 每 种 类 型 都 有 单 精 
度 和 双 精 度 两 种 数据 类 型 ， 而 且 和 之 前 一 样 ， 不 能 由 编译 器 自动 转换 。 
下 面 以 双 精 度 为 例 ， 你 可 以 把 最 后 的 d 改 成 {， 即 得 到 单 精 度 的 数据 结构 。 


“旋转 矩阵 (3x3) : Eigen::Matrix3d。 

“旋转 向 量 (3x 1) : Eigen::AngleAxisd。 
“RHA (3x1) : Eigen::Vector3d。 

“四 元 数 (4x1) : Eigen::Quaterniondo 

“KR ECTS HRFBRE (4x 4) : Eigen::Isometry3d。 
(ARB (4x 4) : Eigen::A ffi ne3do 
射影 变换 (4x 4) : Eigen::Projective3do 


我 们 把 如 何 编译 此 程序 的 问题 交 给 读者 。 在 这 个 程序 中 ， 演 示 了 如 
何 使 用 Eigen 中 的 旋转 和 矩阵、 旋转 向 量 (AngleAxis) 、 欧 拉 角 和 四 元 数 。 


我 们 用 这 几 种 旋转 方式 去 旋转 一 个 向 量 v ， 发 现 结果 是 一 样 的 〈 不 一 样 就 
真是 见鬼 了 ) 。 同 时 ， 也 演示 了 如 何在 程序 中 转换 这 几 种 表达 方式 。 想 
进一步 了 解 Eigen 的 几何 模块 的 读者 可 以 参考 
(http://eigen.tuxfamily.org/dox/group__TutorialGeometry.html) o 


3.7 可视化 演示 


最 后 ， 我 们 为 读者 准备 了 一 个 小 程序 ， 位 于 
slambook/ch3/visualizeGeometry 中 。 它 以 可 视 化 的 形式 演示 了 各 种 表达 方 
式 的 异同 ( 见 图 3-4) 。 读 者 可 以 用 鼠标 操作 一 下 ， 看 看 数据 是 如 何 变 化 
的 。 


图 3-4 ”旋转 矩阵 、 欧 拉 角 、 四 元 数 的 可 视 化 程序 。 


在 这 个 小 程序 中 ， 我 们 在 坐标 原点 放置 了 一 个 彩色 立方 体 。 用 鼠标 
可 以 平移 /旋转 相机 。 你 可 以 实时 地 看 到 相机 姿态 的 变化 。 我 们 显示 了 变 
换 和 矩阵 R,t 、 欧 拉 角 和 四 元 数 的 3 种 姿态 ， 你 可 以 实际 体验 一 下 这 几 个 量 
是 如 何 变 化 的 。 然 而 根据 我 们 的 经 验 ， 除 了 欧 拉 角 之 外 ， 你 应 该 看 不 出 
它们 直观 的 含义 。 

关于 该 程序 的 源 代 码 ， 这 里 就 不 做 解释 了 ， 如 果 你 感 兴 趣 ， 可 以 自 
行 查 看 。 该 程序 的 编译 说 明 请 参照 其 中 的 Readme.txt。 


值得 一 提 的 是 ， 实 际 中 ， 我 们 会 至 少 定义 两 个 坐标 系 : 世界 坐标 系 
和 相机 坐标 系 。 在 该 定义 下 ， 设 某 个 点 在 世界 坐标 系 中 的 坐标 为 p, ， 在 
相机 坐标 系 下 为 p. ， 那 么 : 


Pe = TewPw, (3.40) 
这 里 7， 表示 世界 坐标 系 到 相机 坐标 系 间 的 变换 。 或 者 可 以 用 反 过 来 
ANT: 


we ° 
Pw = £wcePc = To Da (3.41) 


原则 上 ，T、. 和 T ,都 可 以 用 来 表示 相机 的 位 资 ， 事 实 上 它们 也 只 差 
一 个 逆 而 已 。 实 践 当 中 使 用 T, 更 加 常见 ， 而 T ,更 为 直观 。 如 果 把 上 面 


两 式 的 p, 取 成 零 向 量 ， 也 就 是 相机 坐标 系 中 的 原点 ， 那 么 ， 此 时 的 p,, 就 
是 相机 原点 在 世界 坐标 系 下 的 坐标 : 
Pw = Tw0 = twe. (3.42) 


我 们 发 现 这 正 是 T、 的 平移 部 分 。 因 此 ， 可 以 从 T、 中 直接 看 到 相机 
在 何 处 ， 这 也 是 我 们 说 了 ， 更 为 直观 的 原因 。 因 此 ， 在 可 视 化 程序 里 ， 
我 们 显示 了 T ,而 不 是 T „o 

习题 

1. 验 证 旋转 矩阵 是 正 交 和 矩阵 。 

2.* 寻 找 罗 德 里 格 斯 公式 的 推导 过 程 并 加 以 理解 。 


3. 验 证 四 元 数 旋转 某 个 点 后 ， 结 果 是 一 个 虚 四 元 数 ( 实 部 为 零 ) ， 所 
以 仍然 对 应 到 一 个 三 维 空间 点 (503.34) 。 

4. 画 表 总 结 旋 转 和 矩阵 、 轴 和 角 、 欧 拉 角 、 四 元 数 的 转换 关系 。 

5. 假 设 有 一 个 大 的 Eigen 和 矩阵 ， 想 把 它 的 左上 角 3x 3 的 块 取出 来 ， 然 
后 赋值 为 [. , 。 请 编程 实现 。 


6.* 一 般 线性 方程 Ax =b 有 了 哪 几 种 做 法 ? 你 能 在 Eigen 中 实现 吗 ? 


7. 设 有 小 萝卜 一 号 和 小 萝卜 二 号 位 于 世界 坐标 系 中 。 小 萝卜 一 号 的 位 
姿 为 9 ， =[0. 35, 0. 2, 0. 3, 0. 1],t , =[0. 3, 0. 1,0. 1J (q 的 第 一 项 为 实 部 。 
请 把 q 归 一 化 后 再 进行 计算 ) 。 这 里 的 g 和 t 表达 的 是 7 ， 也 就 是 世界 坐 
标 系 到 相机 坐标 系 的 变换 关系 。 小 萝卜 二 号 的 位 次 为 q =[- 0. 5, 0. 4,- 0. 1, 
0. 2],t =[- 0. 1, 0. 5, 0. 317 。 现 在 ， 小 萝卜 一 号 看 到 某 个 点 在 目 身 的 坐标 系 
下 坐标 为 p =[0. 5, 0, 0. 2]* ， 求 该 向 量 在 小 萝卜 二 号 坐标 系 下 的 坐标 。 请 
编程 实现 。 

[1] 正 交 矩阵 即 逆 为 自身 转 置 的 矩阵 。 
[2] 官方 主页 : http:/eigen.tuxfamily.org/index.php?title=Main Page。 
[3] 感 兴趣 的 读者 请 参见 https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula。 


[4] https://en.wikipedia.org/wiki/Gimbal_locko 


第 4 讲 ” 李 和 群 与 李 代数 


主要 目标 


1. 理 解 李 群 与 李 代数 的 概念 ， 掌 握 SO(3), SE(3) 与 对 应 李 代 数 的 表示 
方式 。 


2. 理 解 BCH 近 似 的 意义 。 
3. 学 会 在 李 代 数 上 的 扰动 模型 。 
4. 使 用 Sophus 对 李 代 数 进 行 运算 。 


上 一 讲 ， 我 们 介绍 了 三 维 世界 中 刚体 运动 的 描述 方式 ， 包 括 旋转 答 
阵 、 旋 转向 量 、 欧 拉 角 、 四 元 数 等 若干 种 方式 。 我 们 重点 介绍 了 旋转 的 
表示 ,但 是 在 SLAM 中 ， 除 了 表示 之 外 ， 我 们 还 要 对 它们 进行 估计 和 优 
化 。 因 为 在 SLAM 中 位 姿 是 未 知 的 ， 而 我 们 需要 解决 什么 样 的 相机 位 姿 最 
符合 当前 观测 数据 这 样 的 问题 。 一 种 典型 的 方式 是 把 它 构建 成 一 个 优化 
问题 ， 求 解 最 优 的 R,t ， 使 得 误差 最 小 化 。 


如 前 所 言 ， 旋 转 矩 阵 自 身 是 带 有 约束 的 〈 正 交 且 行列 式 为 1) 。 它 们 
作为 优化 变量 时 ， 会 引入 额外 的 约束 ， 使 优化 变 得 困难 。 通 过 李 群 一 李 
代数 间 的 转换 关系 ， 我 们 希望 把 位 姿 估 计 变 成 无 约束 的 优化 问题 ， 简 化 
求解 方式 。 考 虑 到 读者 可 能 还 没有 李 群 李 代 数 的 基本 知识 ， 我 们 将 从 最 
基本 的 知识 开始 讲 起 。 
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41 ” 李 群 与 李 代 数 基础 


上 一 讲 ， 我 们 介绍 了 旋转 矩阵 和 变换 和 矩阵 的 定义 。 当 时 ， 我 们 说 三 
维 旋转 和 矩阵 构成 了 特殊 正 交 群 SO(3)， 而 变换 矩阵 构成 了 特殊 欧 氏 群 
SE(3): 


SO(3) = {R € R? | RR" = I, det(R) = 1}. (4.1) 
t 

SE(3) = fr = | € R*™4|R € SO(3),t € 3 l (4.2) 
oT 1 


不 过 ， 当 时 我 们 并 未 详细 解释 群 的 含义 。 细 心 的 读者 应 该 会 注意 
到 ， 旋 转 矩 阵 也 好 ， 变 换 矩 阵 也 好 ， 它 们 对 加 法 是 不 封闭 的 。 换 句 话 
说 ， 对 于 任意 两 个 旋转 矩阵 R,,R，， 按 照 矩 阵 加 法 的 定义 ， 和 不 再 是 一 


个 旋转 矩阵 : 


Ri + Ry ¢ SO(3). (4.3) 


对 于 变换 矩阵 亦 是 如 此 。 我 们 发 现 ， 这 两 种 矩阵 并 没有 良好 定义 的 
加 法 ， 相 对 地 ， 它 们 只 有 一 种 较 好 的 运算 : 乘法 。SO(3) 和 SE(3) 关 于 乘法 
是 封闭 的 : 


RRs €SO(3), TT € SE(3). (4.4) 


我 们 知道 ， 乘 法 对 应 着 旋转 或 变换 的 复合 ， 两 个 旋转 矩阵 相 乘 表示 
做 了 两 次 旋转 。 对 于 这 种 只 有 一 个 运算 的 集合 ， 我 们 称 之 为 群 。 


41.1 Æ 


群 (Group) 是 一 种 集合 加 上 一 种 运算 的 代数 结构 。 我 们 把 集合 记 
作 A ， 运 算 记 作 : ， 那 么 群 可 以 记 作 G =(A,: )。 群 要 求 这 个 运算 满足 以 下 
几 个 条 件 : 


1. 封 闭 性 : Va,,a, EA,a ,-a, GA. 


2. 结 合 律 : Va,,a,,a, €A,(a,:a,):a,=a,:(a,-a,). 
3. 么 元 : Ja, EA,s.t. Va EA,a ,-a=a-a,=a. 
4. 逆 : Va EA, Fa ! EAStaa 1=a,. 


读者 可 以 记 作 * 封 结 么 逆 ?” 。 我 们 可 以 验证 ， 旋 转 答 阵 集合 和 和 矩阵 乘 
法 构成 群 ， 同 样 变换 矩阵 和 矩阵 乘法 也 构成 群 (因此 才能 称 它们 为 旋转 
矩阵 群 和 变换 矩阵 群 ) 。 其 他 常见 的 群 包括 整数 的 加 法 (Z, +)， 去 掉 0 后 的 
有 理 数 的 乘法 〈 么 元 为 1) (Q\0,. )， 等 等 。 和 窍 阵 中 常见 的 群 有 : 


一般 线 性 群 GL(n ) 指 nxn 的 可 逆 和 矩阵 ， 它 们 对 和 矩 阵 乘法 成 群 。 

“特殊 正 交 群 SO(n ) 也 就 是 所 谓 的 旋转 矩阵 群 ， 其 中 SO(2) 和 SO(3) 最 
为 常见 。 

“特殊 欧 氏 群 SE(n ) 也 就 是 前 面 提 到 的 n 维 欧 氏 变换 ， 如 SE(2) 和 
SE(3)o 


群 结构 保证 了 在 群 上 的 运算 具有 良好 的 性 质 ， 而 群 论 则 是 研究 群 的 
各 种 结构 和 性 质 的 理论 ， 但 这 里 不 多 加 介绍 。 感 兴趣 的 读者 可 以 参考 任 
意 一 本 近世 代数 教材 。 

李 群 是 指 具 有 连续 (光滑 ) 性 质 的 群 。 像 整数 群 Z 那 样 离散 的 群 没有 
连续 性 质 ， 所 以 不 是 李 群 。 而 SO(n ) 和 SE(n ) 在 实数 空间 上 是 连续 的 。 我 
们 能 够 直观 地 想象 一 个 刚体 能 够 连续 地 在 空间 中 运动 ， 所 以 它们 都 是 李 
群 。 由 于 SO(3) 和 SE(3) 对 于 相机 姿态 估计 尤其 重要 ， 所 以 我 们 主要 讨论 这 
两 个 李 群 。 如 果 读 者 对 李 群 的 理论 性 质感 兴趣 ， 请 参考 文献 [20]。 


下 面 ， 我 们 先 从 较 简 单 的 SO(3) 开 始 讨 论 ， 我 们 将 发 现 每 个 李 群 都 有 
对 应 的 李 代 数 。 我 们 首先 引出 SO(3) 上 面 的 李 代 数 so(3)。 


4.1.2” 李 代数 的 引出 
考虑 任意 旋转 矩阵 R ， 我 们 知道 它 满足 : 


RR! =]. (4.5) 


现在 ， 我 们 说 ，R 是 某 个 相机 的 旋转 ， 它 会 随时 间 连 续 地 变化 ， 即 为 
时 间 的 负数 : R(t )。 由 于 它 仍 是 旋转 和 矩阵， 有 


R(t) R(t)? =I. 
在 等 式 两 边 对 时 间 求 导 ， 得 到 : 
R(t) R(t)’ + R(t) R(t)? = 0. 


整理 得 : 
ROR) = — (RORE) . (4.6) 


AAEE RERE 是 一 个 反对 称 和 矩阵。 回忆 一 下 ， 我 们 在 式 
(3.3) 介绍 叉 积 时 ， 引 入 了 “^ 符 号 ， 将 一 个 向 量变 成 了 反对 称 和 矩阵 。 同 
理 ， 对 于 任意 反对 称 和 矩阵 ， 我 们 亦 能 找到 一 个 与 之 对 应 的 向 量 。 把 这 个 
运算 用 符号 ”表示 : 


0 一 Q3 ag 
a= A= a3 0 一 Q1 AY =a. (4.7) 
—ag ay, 0 


于 是 ， 由 于 'R(OR(iI 是 一 个 反对 称 矩 阵 ， 我 们 可 以 找到 一 个 三 维 
向 量 f (1 )E Ri 520M. FHA: 


R(t) R(t)" = p(t). 


等 式 两 边 右 乘 R (t )， 由 于 R 为 正 交 阵 ， 有 : 


0 -0 02 
R(t)=9(t) R(t) =| 63 0 —d, | RÆ). (4.8) 
—o2 i 0 


可 以 看 到 ， 每 对 旋转 矩阵 求 一 次 导数 ， 只 需 左 乘 一 个 9^ (t ) SBN 
可 。 为 方便 讨论 ， 我 们 设 t ,=0， 并 设 此 时 旋转 矩阵 为 R (0)=T。 按 照 导 数 
定义 ， 可 以 把 R(t ) 在 0 附近 进行 一 阶 泰勒 展开 : 


R(t) = R (to) + R (to) (t — to) 
= I + ¢(to)^ (t). 
我 们 看 到 9 反映 了 R 的 导数 性 质 ， 故 称 它 在 SO(3) 原 点 附近 的 正切 空 
间 (Tangent Space) 上 。 同 时 在 t, 附近 ， 设 9 保持 为 常数 9 (t,)=G,0 ABA 
根据 式 (4.8) ， 有 : 


(4.9) 


R(t) = (to) R(t) = pô R(t). 


上 式 是 一 个 关于 R 的 微分 方程 ， 而 且 知 道 初始 值 R (0)=T ， 解 之 ， 


TF. 
R(t) = exp (Gt). (4.10) 


读者 可 以 验证 上 式 对 微分 方程 和 初始 值 均 成 立 。 不 过 ， 由 于 做 了 一 
定 的 假设 ， 所 以 它 只 在 t=0 附 近 有 效 。 我 们 看 到 ， 旋 转 矩 阵 R 与 另 一 个 反 
对 称 矩 阵 9 ,通过 指数 关系 发 生 了 联系 。 也 就 是 说 ， 当 我 们 知道 某 个 时 刻 
的 R 时 ， 存 在 一 个 向 量 p ， 它 们 满足 这 个 矩阵 指数 关系。 但 是 矩阵 的 指数 
是 什么 呢 ? 这 里 我 们 有 两 个 问题 需要 澄清 : 


1. 如 果 上 式 成 立 ， 那 么 给 定 某 时 刻 的 R ， 我 们 就 能 求 得 一 个 9 ， 它 描 
述 了 R 在 局 部 的 导数 关系 。 与 R 对 应 的 9 有 什么 含义 呢 ? 后 面 会 看 到 ,4 
正 是 对 应 到 SO(3) 上 的 李 代 数 so(3); 

2. 其 次 ， 和 矩阵 指数 exp(@^ ) 如 何 计算 ? 事实 上 ， 这 正 是 李 群 与 李 代 数 
间 的 指数 /对 数 映射 。 

下 面 一 一 加 以 介绍 。 


4.1.3” 李 代数 的 定义 


每 个 李 群 都 有 与 之 对 应 的 李 代 数 。 李 代数 描述 了 李 和 群 的 局 部 性 质 。 
通用 的 李 代 数 的 定义 如 下 : 

李 代 数 由 一 个 集合 V， 一 个 数 域 F 和 一 个 二 元 运算 [, ] 组 成 。 如 果 它 们 
满足 以 下 几 条 性 质 ， 则 称 (V, F, [, ]) 为 一 个 李 人 代数， 记 作 g。 

1. 封 闭 性 VXYEV, [X,Y ]E V. 


2. 双 线性 VX,Y.ZE V,a,b EF, 有 : 


[aX +bY, Z] =a|X, Z] +0b[Y, Z], [Z,aX + bY] = alZ, X]+)[Z,Y]. 


? 


3. ARIZ Wx EV, [X,X ]=0. 

4. 雅 可 比 等 价 VX,YZE V, LX, [YZ 1]+[Z, [X,Y ]]+[Y, [Z,X ]]=0. 

其 中 二 元 运算 被 称 为 李 插 号 。 从 表面 上 来 看 ， 李 代数 所 需要 的 性 质 
还 是 挺 多 的 。 相 比 于 群 中 的 较为 简单 的 二 元 运算 ， 李 括号 表达 了 两 个 元 
素 的 差异 。 它 不 要 求 结合 律 ， 而 要 求 元 素 和 自己 做 李 括 号 之 后 为 零 的 性 
质 。 作 为 例子 ， 三 维 向 量 R? 上 定义 的 叉 积 x 是 一 种 李 括 号 ， 因 此 g=(Rs: ， 
R,x ) 构 成 了 一 个 李 人 代数。 读者 可 以 尝试 将 叉 积 的 性 质 代 入 到 上 面 四 条 性 
质 中 。 


4.1.4” 李 代数 so(3) 


之 前 提 到 的 小， 事实 上 是 一 种 李 代 数 。SO(3) 对 应 的 李 代 数 是 定义 在 
R? 上 的 向 量 ， 我 们 记 作 @。 根 据 前 面 的 推导 ， 每 个 $ 都 可 以 生成 一 个 反 
对 称 和 矩阵 : 


0 -0 $2 
S=¢.=| 6, 0 —¢, | ER. (4.11) 
-9% ġ 0 


FEILER EF, ATA, ,9 ,的 李 括 号 为 


[bi p2] = (P12 一 P281)“. (4.12) 


读者 可 以 验证 该 定义 下 的 李 括 号 满足 上 面 的 几 条 性 质 。 由 于 9 与 反 
对 称 窃 阵 关 系 很 紧密 ， 在 不 引起 层 义 的 情况 下 ， 就 说 so(3) 的 元 素 是 三 维 
向 量 或 者 三 维 反 对 称号 阵 ， 不 加 区 别 : 


5o(3) = {9 € R’,® = p^ €R**?}. (4.13) 


至 此 ， 我 们 已 清楚 了 so(3) 的 内 容 。 它 们 是 一 个 由 三 维 向 量 组 成 的 集 
合 ， 每 个 向 量 对 应 到 一 个 反对 称 和 矩阵 ， 可 以 表达 旋转 答 阵 的 导数 。 它 与 
SO(3) 的 关系 由 指数 映射 给 定 : 


R = exp(¢’). (4.14) 


指数 映射 会 在 稍 后 介绍 。 由 于 已 经 介绍 了 so(3)， 我 们 顺带 先 来 看 
SE(3) 上 对 应 的 李 代 数 。 


4.1.5” 李 代数 se(3) 


对 于 SE(3)， 它 也 有 对 应 的 李 代 数 se(3)。 为 节省 篇 幅 ， 这 里 就 不 介绍 
如 何 引 出 se(3) 了 。 与 so(3) 相 似 ，se(3) 位 于 Rs 空间 中 : 


se(3) = fe a 
9 


我 们 把 每 个 se(3) 元 素 记 作 & ， 它 是 一 个 六 维 向 量 。 前 三 维 为 平移 (但 
含义 与 变换 和 矩 阵 中 的 平移 不 同 ， 分 析 见 后 ) ， 记 作 p ; 后 三 维 为 旋转 ， 记 
作 p ， 实 质 上 是 so(3) 元 素 趾 。 同 时 ， 我 们 拓展 了 “^ 符号 的 含义 。 在 se(3) 
中 ， 同 样 使 用 ^ 符 号 ， 将 一 个 六 维 向 量 转换 成 四 维和 矩阵 ， 但 这 里 不 再 表 
示 反 对 称 : 


A 
E€ Rî, p € RÌ, ọ € so (3), ¿^ = $ d cnet (4.15) 
0 0 


er, (4.16) 


e-| p 
o 0 


RIEA ^M YSR MADEE A <M SB BT” 
关系 ， 以 保持 和 so(3) 上 的 一 致 性 。 读 者 可 以 简单 地 把 se(3) 理 解 成 “由 一 个 
平移 加 上 一 个 so(3) 元 素 构 成 的 向 量 ”( 尽 管 这 里 的 p 还 不 直接 是 平移 ) 。 
同样 ， 李 代数 se(3) 亦 有 类 似 于 so(3) 的 李 括号 : 


lér, E2] = (ETES — ESEN“. (4.17) 


读者 可 以 验证 它 是 否 满 足 李 代数 的 定义 ( 留 作 习题 ) 。 至 此 我 们 已 
经 见 过 两 种 重要 的 李 代 数 so(3) 和 se(3) 了 。 


4.2 ”指数 与 对 数 映射 
4.2.1 SO(3) 上 的 指数 映射 


现在 来 考虑 第 二 个 问题 : exp h^ ) 是 如 何 计算 的 ? 它 是 一 个 和 矩阵 的 指 
数 ， 在 李 群 和 李 代 数 中 ， 称 为 指数 映射 (Exponential Map) 。 同 样 ， 我 
们 会 先 讨 论 so(3) 的 指数 映射 ， 再 讨论 se(3) 的 情形 。 


任意 矩阵 的 指数 映射 可 以 写成 一 个 泰勒 展开 ， 但 是 只 有 在 收敛 的 情 
况 下 才 会 有 结果 ， 其 结果 仍 是 一 个 矩阵 。 
exp(A) = `> DA (4.18) 


eH, Hst ERTA, RIDARE REX EAI AR 
射 : 


exp(¢’) = 2 op)” (4.19) 


我 们 来 仔细 推导 一 下 这 个 定义 。 由 于 9 是 三 维 向 量 ， 我 们 可 以 定义 
它 的 模 长 和 它 的 方向 ， 分 别 记 作 9 和 a ， 于 是 有 p=0a 。 这 里 a 是 一 个 长 
度 为 1 的 方向 向 量 。 首 先 ， 对 于 a^， 有 以 下 两 条 性 质 : 


a^a^ = aa! — I, (4.20) 
以 及 
a^a^a^ = —a”. (4.21) 


读者 可 以 自行 验证 上 述 性 质 。 它 们 提供 了 处 理 a^ 高 阶 项 的 方法 。 利 
用 这 两 个 性 质 ， 我 们 可 以 把 指数 映射 写成 : 


si n 
exp ($^) = exp (fa^) = 》 —(6a") 
n=0 ` 
ae Kj. eg hea 工 03 A ALA 1 yay AN4 
=I +a tea +l a ana + i (ae) +. 


1 


1 
= aa” — a^a^ + 6a + wo oa" 一 yo a^ 一 二 的 (CA) 4 


= aa” + (0 8° + = 6° s) a^ ( 3 02 十 04 s) a^a^ 
= a^a^ + I + sin ĝ0a^ — cos daa‘ 

= (1 — cos 0)a^a^ + I + sin 0a ^ 

= cos ÎI + (1 — cos Jaa” + sin@a”. 


最 后 得 到 一 个 似 兽 相识 的 式 子 : 
exp(Oa^) = cos0T + (1 — cos @)aa' + sina”. (4.22) 


回想 前 一 讲 内 容 ， 它 和 罗 德 里 格 斯 公式 ， 即 式 (3.14) WMa—h x 
表明 ，so(3) 实 际 上 就 是 由 所 谓 的 旋转 向 量 组 成 的 空间 ， 而 指数 映射 即 罗 
德里 格 斯 公式 。 通 过 它们 ， 我 们 把 so(3) 中 任意 一 个 向 量 对 应 到 了 一 个 位 
于 SO(3) 中 的 旋转 矩阵 。 反 之， 如 果 定 义 对 数 映射 ， 也 能 把 SO(3) 中 的 元 
素 对 应 到 so(3) 中 : 


“n +1 


$ Sja (R)“ = bs (—1)” (R =. n) ; (4.23) 


不 过 我 们 通常 不 按照 泰勒 展开 去 计算 对 数 映射 。 在 第 3 讲 中 ， 我 们 已 
经 介绍 过 如 何 根 据 旋转 和 矩阵 计算 对 应 的 李 代数 ， 即 使 用 式 (3.16) ， 利 用 
迹 的 性 质 分 别 求解 转角 和 转轴 ， 采 用 那 种 方式 更 加 省 事 一 些 。 

现在 ， 我 们 介绍 了 指 效 映射 的 计算 方法 。 读 者 可 能 会 问 ， 指 效 映射 
性 质 如 何 呢 ? 是 否 对 于 任意 的 R 都 能 找到 一 个 唯一 的 必 ”很 遗憾 ， 指 数 映 
射 只 是 一 个 满 射 。 这 意味 着 每 个 SO(3) 中 的 元 素 ， 都 可 以 找到 一 个 so(3) 元 
素 与 之 对 应 ; 但 是 可 能 存在 多 个 so(3) 中 的 元 素 ， 对 应 到 同一 个 SO(3)。 至 
少 对 于 旋转 角 9 ， 我 们 知道 多 转 360“ 和 没有 转 是 一 样 的 一 一 它 具 有 周期 
性 。 但 是 ， 如 果 我 们 把 旋转 角度 固定 在 tr 之 间 ， 那 么 李 群 和 李 代 数 元 素 
是 一 一 对 应 的 。 


SO(3) 与 so(3) 的 结论 似乎 在 我 们 的 意料 之 中 。 它 和 我 们 前 面 讲 的 旋转 
向 量 与 旋转 矩阵 很 相似 ， 而 指数 映射 即 是 罗 德 里 格 斯 公式 。 旋 转 和 矩阵 的 
导数 可 以 由 旋转 向 量 指定 ， 指 导 着 如 何在 旋转 和 矩阵 中 进行 微 积 分 运算 。 


4.2.2 ”SE(3) 上 的 指数 映射 


下 面 介 绍 se(3) 上 的 指数 映射 。 为 了 节省 篇 幅 ， 我 们 不 再 像 so(3) 那 样 
详细 推导 指数 映射 。se(3) 上 的 指数 映射 形式 如 下 : 


1 A\n 1 A\n 
pee | = ni (P*) Z mri (P ) P (4.24) 
OT 1 
aJ R JIe| r, (4.25) 
0I 1 


如 果 你 有 耐心， 可 以 照 着 so(3) 上 的 做 法 推导 ， 把 exp 进 行 泰 勒 展开 推 
导 此 式 。 从 结果 上 看 ，& 的 指数 映射 左上 角 的 R 是 我 们 熟知 的 SO(3) 中 的 
元 素 ， 与 se(3) 当 中 的 旋转 部 分 9 对 应 。 而 右上 角 的 则 可 整理 为 Ogo 
=0a) : 


in 0 in 0 1 — cos 
J= sin I+ (1 一 3 aa’ 十 a". (4.26) 


该 式 与 罗 德 里 格 斯 公式 有 些 相似 ， 但 不 完全 一 样 。 我 们 看 到 ， 平 移 
部 分 经 过 指数 映射 之 后 ， 发 生 了 一 次 以 J 为 系数 矩阵 的 线性 变换 。 请 读者 
重视 这 里 的 J ， 因 为 后 面 还 要 用 到 。 


同样 地 ， 虽 然 我 们 也 可 以 类 比 推 得 对 数 映 射 ， 不 过 根据 变换 矩阵 了 求 
so(3) 上 的 对 应 向 量 也 有 更 省 事 的 方式 : 从 左上 角 的 R 计算 旋转 向 量 ， 而 
右上 角 的 t 满足 : 


由 于 J 可 以 由 9 得 到 ， 所 以 这 里 的 p 亦 可 由 此 线性 方程 解 得 。 现 在 ， 
我 们 已 经 弄 清 了 李 群 、 李 代数 的 定义 与 相互 的 转换 关系， 总 结 如 图 4-1 所 


示 。 如 果 读 者 有 哪里 不 明日 ， 可 以 翻 回去 几 页 看 看 公 陈 推导 。 


三 维 旋转 


exp(0 a^) =cos6I+(1—cos@)aa'+sinda® 指数 映射 


对 数 映射 0 = arccos meji Ra=a 
2 
三 维 变换 
李 代数 

exp(é )= exp(¢ ) Jp 

ov i se(3) 

sin 0 SnO + 1-cosO , 

“0 (i 0 | g ”指数 映射 :-| 


A ; tr(R)-1 
对 数 映射 8 = arccos at i Ra=a t=Jp 


图 4-1 SO(3), SE(3), so(3), se(3) 的 对 应 关系 。 


4.3” 李 代数 求 导 与 扰动 模型 
4.3.1 BCH 公 式 与 近似 形式 


使 用 李 代 数 的 一 大 动机 是 进行 优化 ， 而 在 优化 过 程 中 导数 是 非常 必 
要 的 信息 (我 们 会 在 第 6 讲 详细 介绍 ) 。 下 面 来 考虑 一 个 问题 。 昌 然 我 们 
已 经 清楚 了 SO(3) 和 SE(3) 上 的 李 群 与 李 代 数 关 系 ， 但 是 ， 当 在 SO(3) 中 完 
成 两 个 矩阵 乘法 时 ， 李 代数 中 so(3) 上 发 生 了 什么 改变 呢 ? 反 过 来 说 ， 当 
so(3) 上 做 两 个 李 代数 的 加 法 时 ，SO(3) 上 是 否 对 应 着 两 个 矩阵 的 乘积 ? 如 
果 成 立 ， 相 当 于 : 


exp ($1) exp ($2) = exp ((% + $2) ). 


TRO, ,和 ,为 标量 ， 那 显然 该 式 成 立 ; 但 此 处 我 们 计算 的 是 矩阵 的 
指数 函数 ， 而 非 标量 的 指 效 。 换 言 之 ， 我 们 在 研究 下 式 是 否 成 立 : 
In (exp (A) exp(B))=A+B? 


很 遗憾 ， 该 式 在 矩阵 时 并 不 成 立 。 

两 个 李 代数 指数 映射 乘积 的 完整 形式 ， 由 Baker-Campbell-Hausdor ff 
公式 〈BCH 公 式 ) 向 给 出 。 由 于 其 完整 形式 较 复杂 ， 我 们 只 给 出 其 展开 
式 的 前 几 项 : 


In (exp (A) exp (B) = A+ B+ > [A, B] + 5 [A, [A, B]] — = [B,[A, B]]|+--- (4.28) 
其 中 [] 为 李 括 号 。BCH 公 式 告 诉 我 们 ， 当 处 理 两 个 矩阵 指数 之 积 时 ， 

它们 会 产生 一 些 由 李 括 号 组 成 的 余 项 。 特 别 地 ， 考 虑 SO(3) 上 的 李 代 数 

In (exp (6) exp (99))”， 当 9 ,或 9 ,为 小 量 时 ， 小 量 二 次 以 上 的 项 都 可 以 被 忽略 


掉 。 此 时 ，BCH 拥 有 线性 近似 表达 : 


y | IoD hte Mb 
In exp (Gh) exp (GO) md TOD Piton AONE (29) 
J,(o1) gz 十 B81 “doh et. 


以 第 一 个 近似 为 例 。 该 式 告诉 我 们 ， 当 对 一 个 旋转 矩阵 R ，《 李 代数 
为 9 , ) 左 乘 一 个 微小 旋转 矩阵 R ，《〈 李 代数 为 内, ) 时 ， 可 以 近似 地 看 
作 ， 在 原 有 的 李 代数 几 EMES—W(¢,)'¢,. AB, BoE 
摘 述 了 右 乘 一 个 微小 位 移 的 情况 。 于 是 ， 李 代数 在 BCH 近 似 下 ， 分 成 了 
左 乘 近似 和 右 乘 近似 两 种 ， 在 使 用 时 我 们 须 注 意 使 用 的 是 左 乘 模型 还 是 
右 乘 模型 。 

本 书 以 左 乘 为 例 。 左 乘 BCH 近 似 雅 可 比 J 事实 上 就 是 式 (4.26) 的 内 


IN 


上 


in 0 in 0 1 —cos@ 
Jat = rs (1- 5f) aot 4 ean (430) 


它 的 逆 为 


0 0 0 0 0 
J~ =o cot 31t (1 -3 cot 3 aa! 一 50 (4.31) 


而 右 乘 雅 可 比 仅 需要 对 自 变量 取 负 号 即 可 : 
J.(9) = Ji(—¢). (4.32) 


这 样 ， 我 们 融 可 以 谈论 李 群 乘法 与 李 代数 加 法 的 关系 了 。 为 了 方便 
读者 理解 ， 我 们 重新 叙述 一 下 BCH 近 似 的 意义 。 


假定 对 某 个 旋转 R ， 对 应 的 李 代 数 为 9 。 我 们 给 它 左 乘 一 个 微小 旋 
转 ， 记 作 AR ， 对 应 的 李 代 数 为 A 办 。 那 么 ， 在 李 群 上 ， 得 到 的 结果 就 是 
AR.R ， 而 在 李 代 数 上 ， 根 据 BCH 近 似 ， 为 和 大"(%Ao+%。 合并 起 来 ， 可 以 简 
单 地 写成 : 

exp (A^) exp ($^) = exp ( ($ + J, ($) Ag) ^) . (4.33) 

反之 ， 如 果 我 们 在 李 代 数 上 进行 加 法 ， 让 一 个 办 加 上 A 四 ， 那 么 可 以 

近似 为 李 群 上 带 左右 雅 可 比 的 乘法 : 
exp (( + Ad)*) = exp ((JiA@)") exp (9^) = exp ($^) exp ((J-Ap)^). (4.34) 

这 就 为 之 后 李 代 数 上 做 微 积 分 提供 了 理论 基础 。 同 样 地 ， 对 于 

SE(3)， 亦 有 类 似 的 BCH 近 似 公 式 : 
exp (AgE^) exp (E^) = exp (pe 十 é)*) , (4.35) 
exp (£^) exp (AŁ^) ~ exp (I At + ¢)*) . (4.36) 


这 里 形式 比较 复杂 ， 它 是 一 个 6x 6 的 矩阵 ， 读 者 可 以 参考 文献 [6] 


中 式 (7.82) 和 (7.83) 的 内 容 。 由 于 我 们 在 计算 中 不 用 到 该 雅 可 比 ， 故 这 里 略 
去 它 的 实际 形式 。 


4.3.2 ”SO(3) 李 代数 上 的 求 导 


下 面 来 讨论 一 个 融 有 李 代 数 的 水 数 ， 如 何 关 于 该 李 代 数 求 导 的 问 
题 。 该 问题 有 很 强 的 实际 背景 。 在 SLAM 中 ， 我 们 要 估计 一 个 相机 的 位 置 
和 姿态 ， 该 位 容 是 由 SO(3) 上 的 旋转 息 阵 或 SE(3) 上 的 变换 窍 阵 拉 述 的 。 不 
妨 设 某 个 时 刻 小 萝卜 的 位 姿 为 T。 它 观察 到 了 一 个 世界 坐标 位 于 p A, 
产生 了 一 个 观测 数据 z 。 那 么 ， 由 坐标 变换 关系 知 : 


二 (4.37) 


然而 ， 由 于 观测 噪声 w 的 存在 ，z 往往 不 可 能 精确 地 满足 z =Tp 的 天 
系 。 所 以 ， 我 们 通常 会 计算 理想 的 观测 与 实际 数据 的 误差 : 


e=z-—Tp. (4.38) 


假设 一 共有 NN TREN BMRA, FEMMAN TER BA, X 
小 萝卜 的 位 次 估计 ， 相 当 于 是 寻找 一 个 最 优 的 T， 使 得 整体 误差 最 小 化 : 


N 
| 2 
min J(T) = > lz; — Tpi|[5 - (4.39) 


求解 此 问题 ， 需 要 计算 目标 水 数 J 关 于 变换 矩阵 T 的 导数 。 我 们 把 具 
体 的 算法 留 到 后 面 再 讲 。 这 里 重点 要 说 的 是 ， 我 们 经 常会 构建 与 位 姿 有 
关 的 函数 ， 然 后 讨论 该 函数 关于 位 姿 的 导数 ， 以 调整 当前 的 估计 值 。 然 
而 ，SO(3), SE(3) 上 并 没有 展 好 定义 的 加 法 ， 它 们 只 是 群 。 如 果 我 们 把 T 
当成 一 个 普通 和 矩阵 来 处 理 优 化 ， 那 就 必须 对 它 加 以 约束 。 而 从 李 代 数 角 
度 来 说 ， 由 于 李 代 数 由 向 量 组 成 ， 具 有 良好 的 加 法 运算 。 因 此 ， 使 用 李 
代数 解决 求 导 问题 的 思路 分 为 两 种 : 

1. 用 李 代 数 表 示 姿 态 ， 然 后 根据 李 代 数 加 法 来 对 李 代数 求 导 。 

2. 对 李 群 左 乘 RAR 微小 扰动 ， 然 后 对 该 扰动 求 导 ， 称 为 左 扰动 和 
右 扰动 模型 。 

第 一 种 方式 对 应 到 李 代 数 的 求 导 模型 ， 而 第 二 种 则 对 应 到 扰动 模 
型 。 下 面 来 讨论 这 两 种 思路 的 异同 。 


4.3.3 ERARE 


首先 ， 考 虑 SO(3) 上 的 情况 。 假 设 我 们 对 一 个 空间 点 p 进行 了 旋转 ， 
得 到 了 Rp 。 现 在 ， 要 计算 旋转 之 后 点 的 坐标 相对 于 旋转 的 导数 ， 我 们 不 
严谨 地 记 为 怠 : 
0 (Rp) 
OR ` 
由 于 SO(3) 没 有 加 法 ， 所 以 该 导数 无 法 按照 导数 的 定义 进行 计算 。 设 
R 对 应 的 李 代 数 为 由， 我 们 转 而 计算 : 


ð (exp ($^) p) 
Op l 


按照 导数 的 定义 ， 有 : 


a (exp ($) Pp) _ jn Pp (($ + 8p)^) p- exp (^) p 
Od  §b—0 op 
(Cg exp (0%) p— exp (6) p 
5¢>0 op 
(T+ (Fid@)") exp (9^) p ~ exp (4^) p 
~ ôp—0 op 
(J169) exp ($^) p 
54-40 op 
—(exp ($^) p)^ ðh _ 
65920 00 


第 2 行 的 近似 为 BCH 线 性 近似 ， 第 3 行为 泰勒 展开 舍 去 高 阶 项 后 的 近 
似 ， 第 4 行 至 第 5 行将 反对 称 符号 看 作 叉 积 ， 交 换 之 后 变 号 。 于 是 ， 我 们 
推导 出 了 旋转 后 的 点 相对 于 李 代 数 的 导数 : 

o (Rp) 
Op 

不 过 ， 由 于 这 里 仍然 含有 形式 比较 复杂 的 ， 我 们 不 太 希 望 计算 它 。 

而 下 面 要 讲 的 扰动 模型 则 提供 了 更 简单 的 导数 计算 方式 。 


4.3.4 ”扰动 模型 (AR) 


? 
p 


(Rp)^J. 


= (—Rp)^ J. (4.40) 


另 一 种 求 导 方 式 ， 是 对 R 进行 一 次 扰动 AR 。 这 个 扰动 可 以 乘 在 左边 
也 可 以 乘 在 右边 ， 最 后 结果 会 有 一 点 儿 微 小 的 差异 ， 我 们 以 左 扰动 为 
例 。 设 左 扰 动 AR 对 应 的 李 代 效 为 p 。 然 后 ， 对 gq RF, B: 


O(Rp) _ im XP(P’)exp ($*) p- exp ($*) p (4.41) 
Op % 一 0 p l l 
该 式 的 求 导 比 上 面 更 为 简单 : 
O(Rp) _ 1) exp (^ exp (o^) p — exp ($") p 
Oy 9% 一 0 (a) 
~ um LE) epg") p — exp (o^) p 
o 一 0 p 
= lim p Rp = lim wali 2 = —(Rp)”. 
yO p yo p 


可 见 ， 相 比 于 直接 对 李 代 数 求 导 ， 省 去 了 一 个 雅 可 比 .J 的 计算 。 这 使 
得 扰动 模型 更 为 实用 。 请 读者 务必 理解 这 里 的 求 导 运算 ， 这 在 位 姿 估 计 
当中 具有 重要 的 意义 。 


4.3.5 ”SE(3) 上 的 李 代 数 求 导 


最 后 ， 我 们 给 出 SE(3) 上 的 扰动 模型 ， 而 直接 李 代数 上 的 求 导 就 不 再 
介绍 了 。 假 设 某 空 间 点 p 经 过 一 次 变换 7 《对 应 李 代 数 为 5E) ， 得 到 Tp 虽 
。 现 在 ， 给 了 左 乘 一 个 扰动 AT =exp(&" )， 我 们 设 扰 动 项 的 李 代 数 为 65 = 
[6p,69 1 ， 那 么 : 


O(Tp) im exp (SE^) exp (E^) p — exp (£^) p 


OdE — Séo ÔE 
an (I + 6€) exp (E^) p — exp (^) p 
~ um 
6é&—0 ÔE 
人 人 
_ jim 站 xP (E^) P 
6€—0 ÔE 
od dp Rp+t 
. o” 0 1 
~ en ôE 
A 
0 I —(Rp+t)’ 
= lim = eight S (Tp)°. 


我 们 把 最 后 的 结果 定义 成 一 个 算 符 ?路 ， 它 把 一 个 齐 次 坐标 的 空间 
点 变换 成 一 个 4x 6 的 和 矩阵。 


至 此 ， 我 们 已 经 介绍 了 李 群 李 代 数 上 的 微分 运算 。 之 后 的 章节 中 ， 
我 们 将 应 用 这 些 知识 去 解决 实际 问题 。 关 于 李 群 李 代 数 的 某 些 重要 数学 
性 质 ， 我 们 作为 习题 留 给 读者 。 


4.4 SRE: Sophus 


我 们 已 经 介绍 了 李 代 数 的 入 门 知识 ， 现 在 是 通过 实践 演练 巩固 一 下 
所 学 知识 的 机 会 了 。 我 们 来 讨论 如 何在 程序 中 操作 李 代数 。 在 第 3 讲 中 ， 
我 们 看 到 Eigen 提 供 了 几何 模块 ， 但 没有 提供 李 代 数 的 支持 。 一 个 较 好 的 
李 代 数 库 是 Strasdat 维 护 的 Sophus 库 外 。Sophus 库 支持 本 章 主要 讨论 的 
SO(3) 和 SE(3)， 此 外 还 含有 二 维 运动 SO(2), SE(2) 以 及 相似 变换 Sim(3) 的 内 
容 。 它 是 直接 在 Eigen 基 础 上 开发 的 ， 我 们 不 需要 安装 额外 的 依赖 库 。 读 
者 可 以 直接 从 GitHub 上 获取 Sophuse , 或者， 在 本 书 的 代码 目录 
slambook/3rdparty 下 也 提供 了 Sophus 产 代码 。 由 于 历史 原因 ，Sophus 早 期 
版 本 只 提供 了 双 精 度 的 李 群 / 李 代 数 类 。 后 续 版 本 改 瑟 成 了 模板 类 。 模 板 
类 的 Sophus 中 可 以 使 用 不 同 精 度 的 李 群 / 李 人 代数， 但 同时 增加 了 使 用 难 


度 。 本 书 使 用 非 模板 类 的 Sophus 库 。 如 果 读 者 准备 使 用 GitHub 上 的 
Sophus， 请 确保 使 用 的 是 非 模 板 类 的 版 本 。 你 可 以 输入 以 下 命令 获得 非 


模板 类 的 Sophus: 


1 |git clone https://github.com/strasdat/Sophus. git 
2 | cd Sophus 
3 | git checkout a621ff 


本 书 的 3rdparty 中 提供 的 Sophus 也 是 非 模板 版 本 。Sophus 本 身 亦 是 一 
个 cmake 工 程 。 想 必 你 已 经 了 解 如 何 编译 cmake 工 程 了 ， 这 里 不 再 蒙 述 。 


Sophus 库 只 需 编译 即 可 ， 无 须 安装 。 
下 面 来 演示 一 下 Sophus 库 中 的 SO(3) 和 SE(3) 运 算 : 


四 slambook/ch4/useSophus/useSophus.cpp 


#include <iostream> 


#include <cmath> 


using namespace std; 


#include <Eigen/Core> 


#include <Eigen/Geometry> 


#include "sophus/so3.h" 


#include "sophus/se3.h" 


int main( int argc, char** argv ) 


í 


// 沿 Z 轴 转 90 度 的 旋转 矩阵 
Eigen::Matrix3d R = Eigen: :AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)). 


toRotationMatrix(); 

Sophus: :S03 S03_R(R); // Sophus: :SO(3) T vA HARM ik HE KE TE H iE 
Sophus: :S03 SO3_v( 0, 0, M_PI/2 ); // 亦 可 从 旋转 向 量 构造 

Eigen: :Quaterniond q(R); // 或 者 四 元 数 


Sophus::S03 S03_q( q ); 

// 上 述 表 达 方式 都 是 等 价 的 

// 输出 S0(3) 时 ， 以 so(3) 形式 输出 
cout<<"S0(3) from matrix: "<<S03_R<<endl; 
cout<<"S0(3) from vector: "<<S03_v<<endl; 
cout<<"S0(3) from quaternion :"<<S03_q<<end1l; 


// 使 用 对 数 映射 获得 它 的 李 代数 
Eigen::Vector3d so3 = $03_R.log(); 


cout<<"so3 = "<<so3.transpose()<<end1; 

// hat Ay eS) RX Ak 4E E 

cout<<"so3 hat="<<Sophus: :S03: :hat (so3)<<end1; 

// 相对 地 ， vee 为 反对 称 到 向 量 

cout<<"so3 hat vee= "<<Sophus::S03::vee( Sophus::S03::hat(so3) ).transpose()<< 
endl; 


// 增 量 扰动 模型 的 更 新 

Eigen::Vector3d update_so3(1e-4, 0, 0); // 假设 更 新 量 有 这 么 多 
Sophus::S03 S03_updated = Sophus::S03::exp(update_so3)*S03_R; // 左 乘 更 新 
cout<<"S03 updated = "<<S03_updated<<end1; 


oS ESHER HR 8789 分 8) 线 。 ep OSES IEEE AIR/ 

COUt<<" kkKKKRRKKEKK BE DE) 线 RE <<endl ; 

// 对 SE(3) REKA DH 

Eigen: :Vector3d t(1,0,0); // 沿 X 轴 平 移 1 

Sophus::SE3 SE3_Rt(R, t); // 从 R,t 构造 SE(3) 

Sophus::SE3 SE3_qt(q,t); // 从 q,t 构造 SE(3) 

cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl; 

cout<<"SE3 from q,t= "<<end1<<SE3_qt<<end1; 

// 李 代 数 se(3) 是 一 个 六 维 向 量 ， 方 便 起 见 先 typedef 一 下 

typedef Eigen: :Matrix<double,6,1> Vector6d; 

Vector6d se3 = SE3_Rt.log(); 

cout<<"se3 = "<<se3.transpose()<<endl; 

// 观察 和 输出， 会 发 现在 Sophus P, se(3) 平移 在 前 ， 旋 转 在 后 。 与 我 们 的 书 中 是 一 致 
的 。 
// 同样 地 ， 有 hat 和 vee 两 个 运算 符 

cout<<"se3 hat = "<<endl<<Sophus::SE3::hat(se3)<<endl; 

cout<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<< 
endl; 


// 最 后 ， 演 示 一 下 更 新 

Vector6d update_se3; // 更 新 量 

update_se3.setZero(); 

update_se3(0,0) = 1e-4d; 

Sophus: :SE3 SE3_updated = Sophus: :SE3::exp(update_se3)*SE3_Rt; 
cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl; 


return 0; 


该 壮 示 程序 分 为 两 部 分 。 前 半 部 分 介绍 SO(3) 上 的 操作 ， 后 半 部 分 则 
为 SE(3)。 我 们 演示 了 如 何 构 造 SO(3), SE(3) 对 象 ， 对 它们 进行 指数 、 对 数 
映射 ， 以 及 当知 道 更 新 量 后 ， 如 何 对 李 群 元 素 进 行 更 新 。 如 果 读 者 切实 
理解 了 本 讲 内 容 ， 那 么 这 个 程序 对 你 来 说 应 该 没有 什么 难度 。 为 了 编译 
它 ， 请 在 CMakeLists.txt 里 添加 以 下 几 行 : 


四 slambook/ch4/useSophus/CMakeLists.txt 


# 为 使 用 sophus， 需 要 使 用 find_package 命令 找到 它 
find_package( Sophus REQUIRED ) 
include_directories( ${Sophus_INCLUDE_DIRS} ) 


add_executable( useSophus useSophus.cpp ) 
target_link_libraries( useSophus ${Sophus_LIBRARIES} ) 


a a w N = 


人 nd_package 命 令 是 cmake 提 供 的 寻找 某 个 库 的 头 文件 与 库 文件 的 指 
令 。 如 果 cmake 能 够 找到 它 ， 就 会 提供 头 文 件 和 库 文 件 所 在 的 目录 的 变 
© o Ù Sophus 这 个 例子 中 ， 就 是 Sophus INCLUDE _DIRS 和 
Sophus_ LIBRARIES 这 两 个 变量 。 根 据 它们 ， 我 们 就 能 将 Sophus 库 引入 自 
己 的 cmake 工 程 了 。 请 读者 自行 查看 此 程序 的 输出 信息 ， 它 与 我 们 之 前 的 
推导 是 一 致 的 。 


4.5 * 相 似 变 换 群 与 李 代 数 


最 后 ， 我 们 要 提 一 下 在 单 目 视觉 中 使 用 的 相似 变换 群 Sim(3)， 以 及 对 
应 的 李 代 数 sim(3)。 如 果 你 只 对 双 目 SLAM 或 RGB-D SLAM 感 兴趣 ， 可 以 
跳 过 本 节 。 


我 们 已 经 介绍 过 单 目的 尺度 不 确定 性 。 如 果 在 单 目 SLAM 中 使 用 
SE(3) 表 示 位 姿 ， 那 么 由 于 尺度 不 确定 性 与 尺度 漂移 ， 整 个 SLAM 过 程 中 
的 尺度 会 发 生变 化 ， 这 在 SE(3) 中 未 能 体现 出 来 。 因 此 ， 在 单 目 情况 下 我 
们 一 般 会 显 式 地 把 尺度 因子 表达 出 来 。 用 数学 语言 来 说 ， 对 于 位 于 空间 
的 点 p ， 在 相机 坐标 系 下 要 经 过 一 个 相似 变换 ， 而 非 欧 氏 变换 : 


sR t 
p = p= sRp +t. (4.42) 
1 


在 相似 变换 中 ， 我 们 把 尺度 s 表达 了 出 来 。 它 同时 作用 在 p 的 3 个 坐 
标 之 上 ， 对 p 进行 了 一 次 缩放 。 与 SO(3)、SE(3) 相 似 ， 相 似 变换 亦 对 和 矩阵 
乘法 构成 群 ， 称 为 相似 变换 群 Sim(3): 


€ Rs} . (4.43) 


同样 地 ，Sim(3) 也 有 对 应 的 李 代数 、 指 数 映 射 、 对 数 映 射 等 。 李 代数 
sim(3) 元 素 是 一 个 7 维 向 量 z 。 它 的 前 6 维 与 se(3) 相 同 ， 最 后 多 了 一 项 o 。 


p 
sim(3)= 4 ĉl = | 4 wo-| 


oO 


oT + o^ 
2S | ent}, (4.44) 
0 


ol 


它 比 se(3) 多 了 一 项 c。 关 联 Sim(3) 和 sim(3) 的 仍 是 指数 映射 和 对 数 映 
射 。 指 数 映射 为 


er exp (9") Jsp 
exp (6^) = í (4.45) 
07 1 
HAJ 形式 为 
J. Poe 1, - ae? sin + (1 — e7 cos 8) 0 A 
o o? + 0? 
dt e7—1 (e% cos8#—1)o0 + (e7 sin0)0 ahi 
o o? + 0? 


通过 指数 映射 ， 我 们 能 够 找到 李 代 数 与 李 群 的 天 系 。 对 于 李 代 数 元 
素 5 ， 它 与 李 群 的 对 应 关系 为 


s=, R=exp(o"); t = Jsp. (4.46) 


旋转 部 分 和 SO(3) 是 一 致 的 。 平 移 部 分 ， 在 se(3) 中 需要 乘 一 个 雅 可 比 
本， 而 相似 变换 的 雅 可 比 更 复杂 一 些 。 对 于 尺度 因子 ， 可 以 看 到 李 群 中 的 
s 即 为 李 代 数 中 c 的 指数 图 数 。 

Sim(3) 的 BCH 近 似 与 SE(3) 是 类 似 的 。 我 们 可 以 讨论 一 个 点 P 经 过 相 
似 变换 sp 后 ， 相 对 于 s 的 导数 。 同 样 地 ， 存 在 微分 模型 和 扰动 模型 两 种 
方式 ， 而 扰动 模型 较为 简单 。 我 们 省 略 推导 过 程 ， 直 接 给 出 扰动 模型 的 
结果 。 设 给 予 Sp 左 侧 一 个 小 扰动 exp(Z^ )， 并 求 Sp 对 于 扰动 的 导数 。 因 为 
Sp 是 4 维 的 齐 次 坐标 ， 是 7 维 向 量 ， 该 导数 应 该 是 4x 7 的 雅 可 比 。 为 了 方 
便 起 见 ， 记 Sp 的 前 3 维 组 成 向 量 g ， 那 么 : 


I AN 
a | "2 (4.47) 


OC 


关于 Sim(3)， 我 们 就 介绍 到 这 里 。 更 详细 的 关于 Sim(3) 的 资料 ， 建 议 
读者 参见 文献 [21]。 


4.6 h 


本 讲 引 入 了 李 群 SO(3) 和 SE(3)， 以 及 它们 对 应 的 李 代 数 so(3) 和 se(3)。 
我 们 介绍 了 位 姿 在 它们 上 面 的 表达 和 转换 ， 然 后 通过 BCH 的 线性 近似 ， 
我 们 可 以 对 位 姿 进 行 求 导 和 扰动 了 。 这 给 之 后 讲解 位 姿 的 优化 打下 了 理 
论 基础 ， 因 为 我 们 需要 经 常 地 对 某 一 个 位 姿 的 估计 值 进 行 调整 ， 使 它 对 
应 的 误差 碱 小 。 只 有 在 弄 清楚 如 何 对 位 姿 进 行 调整 和 更 新 之 后 ， 我 们 才 
能 继续 下 一 步 的 内 容 。 


可 能 本 讲 的 内 容 比较 偏 理论 化 ， 毕 竟 它 不 像 计算 机 视觉 那样 经 常 有 
好 看 的 图 片 可 以 展示 。 相 比 于 讲解 李 群 李 代数 的 数学 教科 书 ， 由 于 我 们 
只 关心 实用 的 内 容 ， 所 以 讲 的 内 容 非常 精简 ， 速 度 相 对 快 了 一 些 。 请 读 
者 务必 理解 本 章 内 容 ， 它 是 解决 后 续 许多 问题 的 基础 ， 特 别 是 位 姿 估计 
部 分 。 

习题 


1. 验 证 SO(3)、SE(3) 和 Sim(3) 关 于 乘法 成 群 。 
2. 验 证 (Ra , R,x ) 构 成 李 代数 。 
3. 验 证 so(3) 和 se(3) 满 足 李 代数 要 求 的 性 质 。 
4. 验 证 性 质 (4.20) 和 (4.21) 。 
5. 证 明 : 
Rp^R' = (Rp)^. 

6. 证 明 : 

Rexp(p^)R" = exp((Rp)^). 


该 式 称 为 SO(3) 上 的 伴随 性 质 。 同 样 地 ， 在 SE(3) 上 亦 有 伴随 性 质 : 


T exp(€*)T! = exp ((Ad(T)€)’) P (4.48) 
其 中 : 
R AR 
Ad(T) = (4.49) 
0 R 


7. 仿 照 左 扰动 的 推导 ， 推 导 SO(3) 和 SE(3) 在 右 扰动 下 的 导数 。 
8. 搜 索 cmake 的 fi nd_package 指 令 是 如 人 何 运作 的 。 它 有 哪些 可 选 的 参 
数 ? 为 了 让 cmake 找 到 某 个 库 ， 需 要 哪些 先决 条 件 ? 
[1] 谐音 “凤姐 咬 你 ”。 
[2] 自 反 性 是 指 自己 与 自己 的 运算 为 零 。 
[3] 请 注意 有 些 地 方 把 旋转 放 前 面 ， 平 移 放 后 面 ， 也 是 可 行 的 。 
[4] & Whttps://en.wikipedia.org/wiki/Baker-Campbell-Hausdorff_formulac 


[5] 请 注意 这 里 并 不 能 按照 矩阵 微分 来 定义 导数 ， 这 只 是 一 个 记号 。 同 时 ， 在 后 文 极 限 运算 中 ， 严 
说 的 写法 需要 对 分 母 进行 转 置 ， 才 能 得 到 现在 的 结果 。 这 里 为 方便 书写 故 省 去 。 


[6] 请 注意 为 了 使 乘法 成 立 ，p 必须 使 用 齐 次 坐标 。 


[7] 我 会 读 作 “ 降 ”"”， 像 一 个 石子 掉 在 井 里 的 声音 。 
[8] 最 早 提出 李 代 数 的 是 Sophus Lie， 这 个 库 就 以 他 的 名 字 命 名 了 。 


[9] https://github.com/strasdat/Sophuso 


第 5 讲 ” 相 机 与 图 像 


主要 目标 

1. 理 解 针 孔 相 机 的 模型 、 内 参与 径 向 畸变 参数 。 
2. 理 解 一 个 空间 点 是 如 何 投影 到 相机 成 像 平面 的 。 
3. 掌 握 OpenCV 的 图 像 存 储 与 表达 方式 。 

4. 学 会 基本 的 摄像 头 标定 方法 。 


前 面 两 讲 中 ， 我 们 介绍 了 “机 器 人 如 何 表 示 自 身 位 姿 " 的 问题 ， 部 分 
地 解释 了 SLAM 经 典 模型 中 变量 的 含义 和 运动 方程 部 分 。 本 讲 将 讨论 “机 
器 人 如 何 观 测 外 部 世界 *”， 也 就 是 观测 方程 部 分 。 而 在 以 相机 为 主 的 视觉 
SLAM 中 ， 观 测 主要 是 指 相机 成 像 的 过 程 。 


我 们 在 现实 生活 中 能 看 到 大 量 的 照片 。 在 计算 机 中 ， 一 张 照 片 由 很 
多 个 像素 组 成 ， 每 个 像素 记录 了 色彩 或 亮度 的 信息 。 三 维 世界 中 的 一 个 
物体 反射 或 发 出 的 光线 ， 穿 过 相机 光 心 后 ， 投 影 在 相机 的 成 像 平面 上 。 
相机 的 感光 器 件 接收 到 光线 后 ， 产 生 测 量 值 ， 就 得 到 了 像素 ， 形 成 了 我 
们 见 到 的 照片 。 这 个 过 程 能 否 用 数学 原理 来 描述 呢 ? 本 讲 将 首先 讨论 相 
机 模型 ， 说 明 投影 天 系 具体 如 何 描述 ， 相 机 的 内 参 是 什么 。 同 时 ， 简 单 
介绍 双 目 成 像 与 RGB-D 相 机 的 原理 。 然 后 ， 介 绍 二 维 照片 像素 的 基本 操 
作 。 最 后 ， 根 据 内 外 参数 的 含义 ， 演 示 一 个 点 云 拼接 的 实验 。 


5.1 ”相机 模型 


相机 将 三 维 世 界 中 的 坐标 点 (单位 为 米 ， 了 映射 到 二 维 图 像 平面 〈 单 
位 为 像素 ) 的 过 程 能 够 用 一 个 几何 模型 进行 描述 。 这 个 模型 有 很 多 种 ， 
其 中 最 简单 的 称 为 针 孔 模 型 。 针 孔 模 型 是 很 常用 而 且 有 效 的 模型 ， 它 描 
述 了 一 束 光 线 通过 针 孔 之 后 ， 在 针 孔 背面 投影 成 像 的 天 系 。 在 本 书 中 我 
们 用 一 个 简单 的 针 孔 相机 模型 来 对 这 种 映射 关系 进行 建 模 。 同 时 ， 由 于 
相机 镜头 上 的 透镜 的 存在 ， 使 得 光线 投影 到 成 像 平面 的 过 程 中 会 产生 畸 
变 。 因 此 ， 我 们 使 用 针 孔 和 畸变 两 个 模型 来 描述 整个 投影 过 程 。 

在 本 区 先 给 出 相机 的 针 孔 模型 ， 再 对 透镜 的 畸变 模型 进行 讲解 。 这 
两 个 模型 能 够 把 外 部 的 三 维 点 投影 到 相机 内 部 成 像 平 面 ， 构 成 相机 的 内 


E ° 


5.1.1 ” 针 孔 相机 模型 


在 初中 物理 课 浓 上 ， 我 们 可 能 都 见 过 一 个 蜡烛 投影 实验 : 在 一 个 暗 
箱 的 前 方 放 着 一 支 足 燃 的 蜡烛 ， 蜡 烛 的 光 透 过 暗箱 上 的 一 个 小 孔 投 影 在 
暗箱 的 后 方 平 面 上 ， 并 在 这 个 平面 上 形成 一 个 倒立 的 蜡烛 图 像 。 在 这 个 
过 程 中 ， 小 孔 模 型 能 够 把 三 维 世 界 中 的 蜡烛 投影 到 一 个 二 维 成 像 平 面 。 
同 理 ， 我 们 可 以 用 这 个 简单 的 模型 来 解释 相机 的 成 像 过 程 ， 如 图 5-1 所 
示 。 


小 孔 成 像 模型 


相似 三 角形 


相机 坐标 系 O-x-y-z 


图 5-1 ” 针 孔 相机 模型 


现在 来 对 这 个 简单 的 针 孔 模 型 进行 几何 建 模 。 设 O-x-y-z 为 相机 坐标 
系 ， 习 惯 上 我 们 让 z 轴 指 向 相机 前 方 ，x 向 右 ， O 为 摄像 机 的 光 
Ò, ， 也 是 针 孔 模型 中 的 针 孔 。 现 实 世界 的 空间 点 P ， 经 过 小 孔 O 投影 之 
后 ， 落 在 物理 成 像 平面 0' -x -y 上 ， 成 像 点 为 P o 设 P 的 坐标 为 [X,YZ T 
，P 为 [X,Y ,Z J， 并 且 设 物理 成 像 平 面 到 小 孔 的 距离 为 /f (RE) 。 那 
么 ， 根 据 三 角形 相似 关系 ， 有 : 
Gx. Y 
ES (5.1) 
其 中 负 号 表示 成 的 像 是 倒立 的 。 为 了 简化 模型 ， 我 们 可 以 把 成 像 平 
面 对 称 到 相机 前 方 ， 和 三 维 空间 点 一 起 放 在 摄像 机 坐标 系 的 同一 人 出， 如 
图 5-2 的 中 图 所 示 。 这 样 做 可 以 把 公式 中 的 负 号 去 把 ， 使 式 子 更 加 简洁 : 


Z ox 4 es 
f X ye : 
真实 / REFE 对 称 的 成 像 平面 归 一 化 成 人 平面 


图 5-2 ”真实 成 像 平面 ， 对 称 成 像 平面 ， 归 一 化 成 像 平 面 的 图 示 。 


整理 得 : 


(5.3) 


读者 可 能 要 问 ， 为 什么 我 们 可 以 看 似 随意 地 把 成 像 平 面 挪 到 前 方 
Ne? 这 只 是 我 们 处 理 真实 世界 与 相机 投影 的 数学 手段 ， 并 且 ， 大 多 数 相 
机 输出 的 图 像 并 不 是 倒 像 一 一 相机 自身 的 软件 会 帮 你 翻转 这 张 图 像 ， 所 
以 你 看 到 的 一 般 是 正 着 的 像 ， 也 就 是 对 称 的 成 像 平面 上 的 像 。 所 以 ， 尽 
管 从 物理 原理 来 说 ， 小 孔 成 像 应 该 是 倒 像 ， 但 由 于 我 们 对 图 像 作 了 预 处 


理 ， 所 以 理解 成 在 对 称 平面 上 的 像 并 不 会 带 来 什么 坏处 。 于 是 ， 在 不 引 
起 歧义 的 情况 下 ， 我 们 也 不 加 限制 地 称 后 一 种 情况 为 针 孔 模型 。 

式 (5.3) 描述 了 点 P 和 它 的 像 之 间 的 空间 关系 。 不 过 ， 在 相机 中 ， 
我 们 最 终 获 得 的 是 一 个 个 的 像素 ， 这 需要 在 成 像 平 面 上 对 像 进行 采样 和 
量化 。 为 了 描述 传感器 将 感受 到 的 光线 转换 成 图 像 像 素 的 过 程 ， 我 们 设 
在 物理 成 像 平 面 上 固定 着 一 个 像素 平面 o-u-v 。 我 们 在 像素 平面 得 到 了 P 
的 像素 坐标 : [u,v ]'。 

像素 坐标 系 O 通常 的 定义 方式 是 : 原点 o 位 于 图 像 的 左上 角 ，v 轴 向 


右 与 x 轴 平 行 ，v 轴 向 下 与 y 轴 平 行 。 像 素 坐标 系 与 成 像 平面 之 间 ， 相 差 
了 一 个 缩放 和 一 个 原点 的 平移 。 我 们 设 像 素 坐标 在 u 轴 上 缩放 了 a 倍 ， 


在 v 上 缩放 了 5 们 6 同时 ， 原点 平移 了 [c、 ;Cy ig o 
那么 ，P 的 坐标 与 像素 坐标 [wy J 的 关系 为 
' 一 QX' 十 cr 
. (5.4) 
v = BY' + cy 


代入 式 (5.3) 并 把 af EHR- Ep 合并 成 f ， 得 : 


‘esi (5.5) 


v= fut 十 cy 
Eh, 的 单位 为 米 ，wp 的 单位 为 像素 / 米 ， 所 以 f. ,f, 的 单位 为 像 
素 。 把 该 式 写 成 忠 阵 形式 会 更 加 简洁 ， 不 过 左 侧 需要 用 到 齐 次 坐标 : 


u Te 0 6 X 
uv| = 三 却 |0 fy cy Y 
1 0 0 1 Z 


A l 
三 —KP. 5.6 
> (5.6) 


我 们 按照 传统 的 习惯 把 Z 挪 到 左 侧 : 


u fz 0 cœ X 
A 
Z\yu|l= 0 fy Cy Y | =KP. (5.7) 
1 0 0 1 Z 


该 式 中 ， 我 们 把 中 间 的 量 组 成 的 矩阵 称 为 相机 的 内 参数 和 矩阵 
(Cameralntrinsics) 天 。 通 常 认 为 ， 相 机 的 内 参 在 出 厂 之 后 是 固定 的 ， 不 
会 在 使 用 过 程 中 发 生变 化 。 有 的 相机 生产 厂商 会 告诉 你 相机 的 内 参 ， 而 
有 时 需要 你 自己 确定 相机 的 内 参 ， 也 就 是 所 谓 的 标定 。 鉴 于 标定 算法 业 
已 成 熟 ， 且 网 络 上 能 找到 大 量 的 标定 教学 ， 这 里 就 不 介绍 了 。 
有 内 参 ， 自 然 也 有 相对 的 外 参 。 考 虑 到 在 式 (5.6) 中 我 们 使 用 的 是 P 
在 相机 坐标 系 下 的 坐标 。 由 于 相机 在 运动 ， 所 以 P 的 相机 坐标 应 该 是 它 的 
世界 坐标 GCAP,) ， 根 据 相 机 的 当前 位 姿 变 换 到 相机 坐标 系 下 的 结 
果 。 相 机 的 位 姿 由 它 的 旋转 和 矩阵 R 和 平移 向 量 t 来 描述 。 那 么 有 : 


u 


ZPwœw =Z | v | = K (RP, +t) =KTP,. (5.8) 


注意 后 一 个 式 子 隐 含 了 一 次 齐 次 坐标 到 非 齐 次 坐标 的 转换 (你 能 
出 来 吗 ? ) 。 它 描述 了 P 的 世界 坐标 到 像素 坐标 的 投影 关系 。 其 中 ， 相 机 
的 位 姿 R,t 又 称 为 相机 的 外 参数 (Camera Extrinsics) 。 相 比 于 不 变 的 内 
参 ， 外 参 会 随 着 相机 运动 发 生 改 变 ， 同 时 也 是 SLAM 中 待 估计 的 目标 ， 代 
表 着 机 器 人 的 轨迹 。 


上 了 式 两 侧 都 是 齐 次 坐标 。 因 为 齐 次 坐标 乘 上 非 零 常数 后 表达 同样 的 
含义 ， 所 以 可 以 简单 地 把 Z 去 掉 : 


Py = KTP,. (5.9) 
但 这 样 等 号 意义 就 变 了 ， 成 为 在 齐 次 坐标 下 相等 的 概念 ， 相 差 了 一 
个 非 零 常数 。 为 了 避免 麻烦 ， 我 们 还 是 从 传统 意义 上 来 定义 书写 等 号 。 


这 里 还 是 提 一 下 隐 含 着 的 齐 次 到 非 齐 次 的 变换 。 可 以 看 到 ， 右 侧 的 
TP, 表示 把 一 个 世界 坐标 系 下 的 齐 次 坐标 变换 到 相机 坐标 系 下 。 为 了 使 它 


与 K 相 乘 ， 需 要 取 它 的 前 三 维 组 成 向 量 一 一 因为 TP, 最 后 一 维 为 1。 此 


时 ， 对 于 这 个 三 维 向 量 ， 我 们 还 可 以 按照 齐 次 坐标 的 方式 ， 把 最 后 一 维 
进行 归 一 化 处 理 ， 得 到 P 在 相机 归 一 化 平面 上 的 投影 : 


x X/Z 
P= VY | =R P= | Vz ||. (5.10) 
Z 1 


这 时 P. 可 以 看 成 一 ee 
机 前 方 z =1 处 的 平面 上 。 该 平面 称 为 归 一 化 平面 。 由 于 P. 经 过 内 参 之 后 


RE 
的 点 进行 量化 测量 的 结果 。 


至 此 ， 针 孔 相 机 的 成 像 模 型 我 们 就 讲 清楚 了 。 
5.1.2 ”畸变 


为 了 获得 好 的 成 像 效 果 ， 我 们 在 相机 的 前 方 加 了 透镜 。 透镜 的 加 入 
对 成 像 过 程 中 光线 的 传播 会 产生 新 的 影响 : 一 是 透镜 自身 的 形状 对 光线 
传播 的 影响 ， 二 是 在 机 械 组 装 过 程 中 ， 透 镜 和 成 像 平 面 不 可 能 完全 平 
行 ， 这 也 会 使 得 光线 穿 过 透镜 投影 到 成 像 面 时 的 位 置 发 生变 化 。 


由 透镜 形状 引起 的 畸变 称 为 径 向 畸变 。 在 针 孔 模型 中 ， 一 条 直线 投 
影 到 像素 平面 上 还 是 一 条 直线 。 可 是 ， 在 实际 担 摄 的 照片 中 ， 摄 像 机 的 
透镜 往往 使 得 真实 环境 中 的 一 条 直线 在 图 片 中 变 成 了 曲线 ”。 越 靠近 图 
像 的 边缘 ， 这 种 现象 越 明 显 。 由 于 实际 加 工 制作 的 透镜 往往 是 中 心 对 称 
的 ， 这 使 得 不 规则 的 畸变 通常 径 向 对 称 。 它 们 主要 分 为 两 大 类 : AVE 
变 和 枕 形 畸变 ， 如 图 5-3 所 示 。 


Ff 
F 
T 
H 
may 


正常 图 像 


图 5-3 ” 径 向 畸变 的 两 种 类 型 。 


桶 形 畸 变 是 由 于 图 像 放 大 率 随 着 与 光 轴 之 间 的 距离 增加 而 减 小 ， 而 
枕 形 畸 变 则 恰好 相反 。 在 这 两 种 畸变 中 ， 穿 过 图 像 中 心 和 光 轴 有 交点 的 
直线 还 能 保持 形状 不 变 。 

除了 透镜 的 形状 会 引入 径 向 畸变 外 ， 在 相机 的 组 装 过 程 中 由 于 不 能 
使 透镜 和 成 像 面 严格 平行 也 会 引入 切 向 畸变 ， 如 图 5-4 所 示 。 


摄像 头 传感器 
5-4 切 向 畸变 来 源 示意 图 。 


为 更 好 地 理解 径 向 畸变 和 切 向 畸变 ， 我 们 用 更 严格 的 数学 形式 对 两 
者 进行 描述 。 我 们 知道 ， 平 面 上 的 任意 一 点 p 可 以 用 笛 卡 儿 坐 标 表 示 为 
[xy] 了 ， 也 可 以 把 它 写 成 极 坐标 的 形式 [9 ]* ， 其 中 r 表示 点 p 与 坐标 系 原 
点 之 间 的 距离 ，9 表示 与 水 平 轴 的 夹 角 。 径 向 畸变 可 看 成 坐标 点 沿 着 长 度 
方向 发 生 了 变化 6r ， 也 就 是 其 距离 原点 的 长 度 发 生 了 变化 。 切 向 畸变 5 
以 看 成 坐标 总 沿 着 切线 方向 发 生 了 变化 ， 也 就 是 水 平 夹 角 发 生 了 变化 6 


(0) 


对 于 径 向 畸变 ， 无 论 是 桶 形 畸 变 还 是 枕 形 畸 变 ， 由 于 它们 都 是 随 着 
与 中 心 之 间 的 距离 增加 而 增加 ， 因 此 可 以 用 一 个 多 项 式 函 数 来 描述 畸变 
前 后 的 坐标 变化 : 这 类 畸变 可 以 用 与 距 中 心 的 距离 有 关 的 二 次 及 高 次 多 
项 式 函 数 进 行 纠正 。 其 中 [xy 开 是 未 纠正 的 点 的 坐标 ，[EX corrected yy conecea 1 
是 纠正 后 的 点 的 坐标 。 注 意 ， 它 们 都 是 归 一 化 平面 上 的 点 ， 而 不 是 像素 
平面 上 的 点 。 


心 corrected 一 x(1 + kır? + karf + k3r®) (5 11) 


Ycorrected = y(1 + kyr? + kort + k3r®) 

在 式 (5.11) 描 述 的 纠正 模型 中 ， 对 于 畸变 较 小 的 图 像 中 心 区 域 ， 畸 变 
纠正 主要 是 k , 起 作用 ; 而 对 于 畸变 较 大 的 边缘 区 域 ， 主 要 是 k , 起 作用 。 
普通 摄像 头 用 这 两 个 系数 就 能 很 好 地 纠正 径 向 畸变 。 对 畸变 很 大 的 摄像 
头 ， 比 如 鱼 眼 镜头 ， 可 以 加 入 k , 畸变 项 对 畸变 进行 纠正 。 

另 一 方面 ， 对 于 切 向 畸变 ， 可 以 使 用 另外 的 两 个 参数 p , ,p ,来 进行 
纠正 : 


Tcorrected = L 十 2pı Ty + po(r? F 2x?) (5 12) 


Ycorrected = Y + P1 (r? 下 2y?) + 2p2ry 


因此 ， 联 合式 (5.11) 和 式 (5.12)， 对 于 相机 坐标 系 中 的 一 点 P (XYZ )， 
我 们 能 够 通过 5 个 畸变 系数 找到 这 个 点 在 像素 平面 上 的 正确 位 置 : 


1. 将 三 维 空间 点 投影 到 归 一 化 图 像 平 面 。 设 它 的 归 一 化 坐标 为 [x,y J 


2. 对 归 一 化 平面 上 的 点 进行 径 向 畸变 和 切 向 畸变 纠正 。 


X corrected = x(1 aS kır? F kor* A kar°) 十 2pı £y + p2(r? 十 272) (5 13) 
Yeorrected = Y(1 + kır? + kort + kgr®) + pi(r? + 2y?) + 2poxy 
3. 将 纠正 后 的 点 通过 内 参数 矩阵 投影 到 像素 平面 ， 得 到 该 点 在 图 像 上 
的 正确 位 置 。 


1 = Veo Uconseted + Cr (5 14) 


C= fy Ycorrected + Cy 


在 上 面 的 纠正 畸变 的 过 程 中 ， 我 们 使 用 了 5 个 畸变 项 。 实 际 应 用 中 ， 
可 以 灵活 选择 纠正 模型 ， 比 如 只 选择 k , ,p .p, 这 3 项 等 。 


在 这 一 节 中 ， 我 们 对 相机 的 成 像 过 程 使 用 针 孔 模型 进行 了 建 模 ， 也 
对 透镜 5 引起 的 径 向 畸变 和 切 向 畸变 进行 了 描述 。 实 际 的 图 像 系统 中 ， 学 
者 们 提出 了 很 多 其 他 的 模型 ， 比 如 相机 的 仿 射 模型 和 透视 模型 等 ， 同 时 
也 存在 很 多 其 他 类 型 的 畸变 。 考 虑 到 视觉 SLAM 中 一 般 都 使 用 普通 的 摄像 
头 ， 有 针 孔 模型 及 径 向 畸变 和 切 向 畸变 模型 已 经 足够 ， 因 此 ， 我 们 不 再 
对 其 他 模型 进行 掏 述 。 

值得 一 提 的 是 ， 存 在 两 种 去 畸变 处 理 《Undistort， 或 称 畸 变 校正 ) 做 
法 。 我 们 可 以 选择 先 对 整 张 图像 进 行 去 畸变 ， 得 到 去 畸变 后 的 图 像 ， 然 
后 讨论 此 图 像 上 的 点 的 空间 人 位置。 或者， 也 可 以 先 考 虑 图 像 中 的 某 个 
点 ， 然 后 按照 去 畸变 方程 ， 讨 论 其 去 畸变 后 的 空间 位 置 。 二 者 都 是 可 行 
的 ， 不 过 前 者 在 视觉 SLAM 中 似乎 更 加 常见 一 些 。 所 以 ， 当 一 个 图 像 去 了 畸 
变 之 后 ， 我 们 就 可 以 直接 用 针 孔 模型 建立 投影 关系 ， 而 不 用 考虑 畸变 
了 。 因 此 ， 在 后 文 的 讨论 中 ， 我 们 可 以 直接 假设 图 像 已 经 进行 了 去 畸变 
处 理 。 


最 后 ， 我 们 小 结 一 下 单 目 相机 的 成 像 过 程 : 

1. 首 先 ， 世 界 坐标 系 下 有 一 个 固定 的 点 P ， 世 界 坐标 为 P, 。 

2. 由 于 相机 在 运动 ， 它 的 运动 由 Rt 或 变换 矩阵 TGSE(3) 描 述 。P 的 
相机 坐标 为 B= RP, +t. 

3. 这 时 的 人 仍 有 X,YZ 三 个 量 ， 把 它们 投影 到 归 一 化 平面 Z=1 上 ， 得 
到 P 的 归 一 化 相机 坐标 : P. =[X/Z.Y/Z, 1] 10 o 

4. 最 后 ，P 的 归 一 化 坐标 经 过 内 参 后 ， 对 应 到 它 的 像素 坐标 : P， 
=KP_ o 

综 上 所 述 ， 我 们 一 共 谈 到 了 四 种 坐标 : 世界 坐标 、 相 机 坐标 、 归 一 
化 相机 坐标 和 像素 坐标 。 请 读者 厘清 它们 的 关系 ， 它 反映 了 整个 成 像 的 


过 程 。 


5.1.3” 双 目 相 机 模型 


针 孔 相机 模型 描述 了 单个 相机 的 成 像 模型 。 然 而 ， 仅 根据 一 个 像 
素 ， 我 们 是 无 法 确定 这 个 空间 点 的 具体 位 置 的 。 这 是 因为 ， 从 相机 光 心 
到 归 一 化 平面 连 线 上 的 所 有 点 ， 都 可 以 投影 至 该 像素 上 。 只 有 当 P 的 深度 
确定 时 〈 比 如 通过 双 目 或 RGB-D 相 机 ) ， 我 们 才能 确切 地 知道 它 的 空间 
位 置 。 如 图 5-5 所 示 。 


归 一 化 北面 


区 三 ] 


若 P 的 距离 已 知 ， 则 位 置 可 以 确定 


A 标 Z 
A EMN AR 


相机 坐标 系 


像素 点 P 可 能 的 位 置 


y 归 一 化 坐标 系 
图 5-5 “像素 点 可 能 存在 的 位 置 。 


测量 像素 距离 (RE) 的 方式 有 很 多 种 ， 像 人 眼 就 可 以 根据 左右 
眼看 到 的 景物 差异 (或 称 视差 ) 来 判断 物体 与 我 们 之 间 的 距离 。 双 目 相 
机 的 原理 亦 是 如 此 : 通过 同步 采集 左右 相机 的 图 像 ， 计 算 图 像 间 视差 ， 
来 估计 每 一 个 像素 的 深度 。 下 面 简单 讲 讲 双 目 相 机 的 成 像 原理 (如 图 5-6 
所 示 ) 。 


双 目 相机 一 般 由 左 眼 相机 和 右 眼 相机 两 个 水 平 放置 的 相机 组 成 。 当 
然 也 可 以 做 成 上 下 两 个 目 上 由 ， 不 过 我 们 见 到 的 主流 双 目 都 是 做 成 左右 形 
式 的 。 在 左右 双 目 相机 中 ， 我 们 可 以 把 两 个 相机 都 看 作 针 孔 相 机 。 它 们 
是 水 平 放 置 的 ， 意 味 着 两 个 相机 的 光圈 中 心 都 位 于 x 轴 上 。 两 者 之 间 的 距 
离 称 为 双 目 相机 的 基线 (Baseline， 记 作 b ) ， 是 双 目 相机 的 重要 参数 。 


左 眼 相机 右 眼 相机 
图 5-6 ” 双 目 相机 的 成 像 模 型 。O , ,O , 为 左右 光圈 中 心 ， 方 框 为 成 像 平 面 ，f 为 焦距 。u , 和 
un 为 成 像 平面 的 坐标 。 请 注意 ， 按 照 图 中 坐标 定义 ，u , 应 该 是 负数 ， 所 以 图 中 标 出 的 距离 
为 -u RO 


现在 ， 考 虑 一 个 空间 点 P ， 它 在 左 眼 相机 和 右 眼 相机 各 成 一 像 ， 记 作 
Pi ,PR 。 由 于 相机 基线 的 存在 ， 这 两 个 成 像 位 置 是 不 同 的 。 理 想 情 况 下 ， 


由 于 左右 相机 只 在 x 轴 上 有 位 移 ， 因 此 P 的 像 也 只 在 x 轴 (对 应 图 像 的 u 
轴 ) 上 有 差异 。 记 它 的 左 侧 坐 标 为 u, ， 右 侧 坐 标 为 u; 。 那 么 ， 其 几何 关 


系 如 图 5-6 右 侧 所 示 。 根 据 APP, P, MAPO, O 的 相似 关系 ， 有 : 


z—f b—u,r+ur 


b (5.15) 
稍 加 整理 ， 得 : 
z=}, d = ur — UR. (5.16) 


XE, d 为 左右 图 的 横 坐 标 之 差 ， 称 为 视差 (Disparity) 。 根 据 视 
差 ， 我 们 可 以 估计 一 个 像素 与 相机 之 间 的 距离 。 视 差 与 距离 成 有 反比: 视 
差 越 大 ， 距 离 越 近 中 。 同 时 ， 由 于 视差 最 小 为 一 个 像素 ， 于 是 双 目 的 深 
度 存在 一 个 理论 上 的 最 大 值 ， 由 fb 确定 。 我 们 看 到 ， 当 基线 越 长 时 ， 双 
目 能 测 到 的 最 大 距离 就 会 越 远 ， 反 之 ， 小 型 双 目 器 件 则 只 能 测量 很 近 的 
距离 。 

虽然 由 视差 计算 深度 的 公式 很 简洁 ， 但 视差 本 身 的 计算 却 比较 困 
难 。 我 们 需要 确切 地 知道 左 眼 图 像 某 个 像素 出 现在 右 眼 图 像 的 哪 一 个 位 


E 〈《 即 对 应 关系 ) ， 这 件 事 亦 属于 人 类 觉得 容易 而 计算 机 觉得 困难 ”的 
任务 。 当 我 们 想 计算 每 个 像素 的 深度 时 ， 其 计算 量 与 精度 都 将 成 为 问 
题 ， 而 且 只 有 在 图 像 纹 理 变 化 丰富 的 地 方才 能 计算 视差 。 由 于 计算 量 的 
原因 ， 双 目 深 度 估计 仍 需 要 使 用 GPU 或 FPGA 来 计算 。 这 将 在 第 13 讲 中 提 


到 。 


5.1.4 RGB-D 相 机 模型 


相 比 于 双 目 相机 通过 视差 计算 深度 的 方式 ，RGB-D 相 机 的 做 法 更 为 
“主动 ”一 些 ， 它 能 够 主动 测量 每 个 像素 的 深度 。 目 前 的 RGB-D 相 机 按 原 
理 可 分 为 两 大 类 ( 见 图 5-7) : 

1. 通 过 红外 结构 光 (Structured Light) 来 测量 像素 距离 的 。 例 子 有 
Kinect 1 代 、Project Tango 1 代 、Intel RealSense 等 。 


2. 通 过 飞行 时 间 法 (Time-of- flight，ToF) 原理 测量 像素 距离 的 。 例 
子 有 Kinect 2 代 和 一 些 现 有 的 ToF 传 感 器 等 。 


结构 光 原 理 


飞行 时 间 原 理 
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ARME 结构 光 接收 器 


DE Aa ait 脉冲 光 接 收 需 


红外 结构 光 发 射 器 


RGB 摄像 头 
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图 5-7 RGB-D 相 机 原理 示意 图 


无 论 是 哪 种 类 型 ，RGB-D 相 机 都 需要 向 探测 目标 发 射 一 束 光线 ( 通 
常 是 红外 光 ) 。 在 结构 光 原 理 中 ， 相 机 根据 返回 的 结构 光 图 案 ， 计 算 物 
体 与 上 自身 之 间 的 距离 。 而 在 ToF 原 理 中 ， 相 机 向 目标 发 射 脉冲 光 ， 然 后 根 
据 发 送 到 返回 之 间 的 光束 飞行 时 间 ， 确 定 物体 与 自身 之 间 的 距离 。ToF 原 
理 和 激光 传感器 十 分 相似 ， 只 不 过 激光 是 通过 了 逐 点 扫描 来 获取 距离 ， 而 
ToF 相 机 则 可 以 获得 整个 图 像 的 像素 深度 ， 这 也 正 是 RGB-D 相 机 的 特 后 。 
所 以 ， 如 果 你 把 一 个 RGB-D 相 机 拆 开 ， 通 常会 发 现 除 了 普通 的 摄像 头 之 
外 ， 至 少 会 有 一 个 发 射 器 和 一 个 接收 器 。 


在 测量 深度 之 后 ，RGB-D 相 机 通常 按照 生产 时 的 各 相机 摆 放 位 置 ， 
自己 完成 深度 与 彩色 图 像素 之 间 的 配对 ， 输 出 一 一 对 应 的 彩色 图 和 深度 
图 。 我 们 可 以 在 同一 个 图 像 位 置 ， 读 取 到 色彩 信息 和 距离 信息 ， 计 算 像 
素 的 3D 相 机 坐标 ， 生 成 点 云 (Point Cloud) 。 对 RGB-D 数 据 ， 既 可 以 在 
图 像 层 面 进行 处 理 ， 亦 可 在 点 云层 面 处 理 。 本 讲 的 第 二 个 实验 将 演示 
RGB-D 相 机 的 点 云 构 建 过 程 。 


RGB-D 相 机 能 够 实时 地 测量 每 个 像素 点 的 距离 。 但 是 ， 由 于 这 种 发 
射 -接收 的 测量 方式 ， 其 使 用 荡 围 比较 受 限 。 用 红外 光 进 行 深 度 值 测量 的 
RGB-D 相 机 ， 容 易 受 到 日 光 或 其 他 传感器 发 射 的 红外 光 干 扰 ， 因 此 不 能 
在 室外 使 用 ， 同 时 使 用 多 个 时 也 会 相互 干扰 。 对 于 透射 材质 的 物体 ， 因 
为 接收 不 到 反射 光 ， 所 以 无 法 测量 这 些 点 的 位 置 。 此 外 ，RGB-D 相 机 在 
RMA. HA, BALES. 
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相机 加 上 镜头 ， 把 三 维 世界 中 的 信息 转换 成 了 一 张 由 像素 组 成 的 照 
片 ， 随 后 存储 在 计算 机 中 ， 作 为 后 续 处 理 的 数据 来 产 。 在 数学 中 ， 图 像 
可 以 用 一 个 和 矩阵 来 描述 ;而 在 计算 机 中 ， 它 们 占据 一 段 连 续 的 磁盘 或 内 
存 空间 ， 可 以 用 二 维 数组 来 表示 。 这 样 一 来 ， 程 序 就 不 必 区 别 它 们 处 理 
的 是 一 个 数值 矩阵 ， 还 是 有 实际 意义 的 图 像 了 。 


本 节 ， 我 们 将 介绍 计算 机 图 像 处 理 的 一 些 基 本 操作 。 特 别 地 ， 通 过 
OpenCV 中 图 像 数据 的 处 理 ， 理 解 计 算 机 中 处 理 图 像 的 常见 步骤 ， 为 后 续 
章节 打下 基础 。 


计算 机 中 图 像 的 表示 

我 们 从 最 简单 的 图 像 一 一 灰 度 图 说 起 。 在 一 张 灰 度 图 中 ， 每 个 像素 
位 置 (x,y ) 对 应 一 个 灰 度 值 [ ， 所 以 ， 一 张 宽度 为 w、 高 度 为 h 的 图 像 ， 数 
学 上 可 以 记 为 一 个 矩阵: 

I(x, y) € RY*". 

然而 ， 计 算 机 并 不 能 表达 整个 实数 空间 ， 所 以 我 们 只 能 在 某 个 范围 
内 对 图 像 进行 量化 。 例 如 ， 常 见 的 灰 度 图 中 ， 用 0~255 的 整数 〈 即 一 个 
unsigned char，1 个 字 节 ) 来 表达 图 像 的 灰 度 大 小 。 那 么 ， 一 张 宽度 为 640 
像素 、 高 度 为 480 像 素 分 辩 率 的 灰 度 图 就 可 以 表示 为 


1 | unsigned char image[480] [640] ; 


为 什么 这 里 的 二 维 数 组 是 480x 640 呢 ? 因为 在 程序 中 ， 图 像 以 二 维 
数组 形式 存储 。 它 的 第 一 个 下 标 是 指数 组 的 行 ， 而 第 二 个 下 标 则 是 列 。 


在 图 像 中 ， 数 组 的 行 数 对 应 图 像 的 高 度 ， 而 列 数 对 应 图 像 的 宽度 。 
下 面 考察 这 幅 图 像 的 内 容 。 图 像 目 然 是 由 像素 组 成 的 。 当 访问 某 一 
个 像素 时 ， 需 要 指明 它 所 处 的 坐标 ， 如 图 5-8 所 示 。 


原点 O x X Hh WB, 宽度 ) 像素 的 具体 值 


灰 度 图 : 0~255 8 位 整数 
深度 图 : 0~65535 16 位 整数 
彩色 图 : 多 通道 


(BGR、RGB、RGBA 等 ) 


引 clR13lcla 


| 一 24 位 一 


- 幅 图 像 
图 5-8 ”图 像 坐 标示 意图 。 


图 5-8 左 边 显 示 了 传统 像素 坐标 系 的 定义 方式 。 像 素 坐标 系 原 点 位 于 
图 像 的 左上 角 ，X 轴 向 右 ，Y 轴 向 下 (也 就 是 前 面 所 说 的 u,v Ain) 。 如 
果 它 还 有 第 三 个 轴 一 一 Z 轴 ， 那 么 根据 右手 法 则 ，Z 轴 应 该 是 向 前 的 。 这 
种 定义 方式 是 与 相机 坐标 系 一 致 的 。 我 们 平时 说 的 图 像 的 宽度 或 列 数 ， 
对 应 着 X 轴 ; 而 图 像 的 行 数 或 高 度 ， 则 对 应 着 它 的 了 7 轴 。 


根据 这 种 定义 方式 ， 如 果 我 们 讨论 一 个 位 于 xy 处 的 像素 ， 那 么 它 在 
程序 中 的 访问 方式 应 该 是 


1 | unsigned char pixel = image[y] [x]; | 


它 对 应 着 灰 度 值 1 (x,y ) 的 读数 。 请 注意 这 里 的 x 和 y 的 顺序 。 虽 然 我 
们 不 大 其 烦 地 讨论 坐标 系 的 问题 ， 但 是 像 这 种 下 标 顺 序 的 错误 ， 仍 会 是 
新 手 在 调试 过 程 中 经 党 碰 到 的 ， 且 具有 一 定 隐 贡 性 的 错误 之 一 。 如 果 你 
在 写 程 序 时 不 愤 调 换 了 x,y 的 坐标 ， 编 译 器 无 法 提供 任何 信息 ， 而 你 所 能 
看 到 的 只 是 程序 运行 中 的 一 个 越界 错误 而 已 。 

一 个 像素 的 灰 度 可 以 用 8 位 整数 记录 ， 也 就 是 一 个 0~255 的 值 。 当 我 
们 要 记录 的 信息 更 多 时 ， 一 个 字 节 恺 怕 就 不 够 了 。 例 如 ， 在 RGB-D 相 机 
的 深度 图 中 ， 记 录 了 各 个 像素 与 相机 之 间 的 距离 。 这 个 距离 通常 是 以 毫 
米 为 单位 ， 而 RGB-D 相 机 的 量程 通常 在 十 几米 左右 ， 超 过 了 255。 这 时 ， 
人 们 会 采用 16 位 整数 (C++ 中 的 unsigned short) 来 记录 深度 图 的 信息 ， 也 


就 是 位 于 0 人 65536 的 值 。 换 算 成 米 的 话 ， 最 大 可 以 表示 65 米 ， 足 够 RGB- 
D 相 机 使 用 了 。 


彩色 图 像 的 表示 则 需要 通道 (channel) 的 概念 。 在 计算 机 中 ， 我 们 
用 红色 、 绿 色 和 蓝 色 这 三 种 颜色 的 组 合 来 表达 任意 一 种 色彩 。 于 是 对 于 
每 一 个 像素 ， 就 要 记录 其 R、G、B 三 个 数值 ， 每 一 个 数值 就 称 为 一 个 通 
道 。 例 如 ， 最 常见 的 彩色 图 像 有 三 个 通道 ， 每 个 通道 都 由 8 位 整数 表示 。 
在 这 种 规定 下 ， 一 个 像素 占据 24 位 空间 。 


通道 的 数量 、 顺 序 都 是 可 以 自由 定义 的 。 在 OpenCV 的 彩色 图 像 中 ， 
通道 的 默认 顺序 是 B、G、R。 也 就 是 说 ， 当 我 们 得 到 一 个 24 位 的 像素 
时 ， 前 8 位 表示 蓝 色 数值 ， 中 间 8 位 为 绿色 ， 最 后 8 位 为 红色 。 同 理 ， 亦 可 
使 用 R、G、B 的 顺序 表示 一 个 彩色 图 。 如 果 还 想 表 达 图 像 的 透明 度 ， 就 
使 用 R、G、B、A 四 个 通道 。 


5.3 ”实践 : 图 像 的 存 取 与 访问 


下 面 通过 一 个 演示 程序 来 理解 ， 在 OpenCV 中 图 像 是 如 何 存 取 ， 而 我 
们 又 是 如 何 访问 其 中 的 像素 的 。 


5.3.1 ”安装 OpenCV 


OpenCV” 提供 了 大 量 的 开源 图 像 算法 ， 是 计算 机 视觉 中 使 用 极 广 的 
图 像 处 理 算法 库 。 本 书 也 使 用 OpenCV 做 基本 的 图 像 处理 。 在 使 用 之 前 ， 
建议 读者 从 源 代码 安装 它 。 在 Ubuntu 下 ， 有 从 源 代码 安装 和 只 安装 库 文 
件 两 种 方式 可 以 选择 : 


1. 从 源 代 码 安 装 ， 是 指 从 OpenCV 网 站 下 载 所 有 的 OpenCV 源 代码 ， 并 
在 机 器 上 编译 安装 ， 以 便 使 用 。 好 处 是 可 以 选择 的 版 本 比较 丰富 ， 而 且 
能 看 到 源 代码 ， 不 过 需要 花费 一 些 编译 时 间 ]。 

2. 只 安装 库 文 件 ， 是 指 通过 Ubuntu 来 安装 由 Ubuntu 社区 人 员 已 经 编译 
好 的 库 文 件 ， 这 样 就 无 须 重 新 编译 一 遍 。 

由 于 我 们 使 用 较 新 版 本 的 OpenCV， 所 以 必须 从 源 代 码 来 安装 它 。 一 
来 ， 可 以 调整 一 些 编译 选项 ， 匹 配 编程 环境 (例如 ， 需 不 需要 GPU 加 速 
等 ) ; 再 者 ， 源 代码 安装 可 以 使 用 一 些 额外 的 功能 。OpenCV 目 前 维护 了 


两 个 主要 版 本 ， 分 为 OpenCV 2.4 系 列 和 OpenCV 3 系列 。 本 书 使 用 
OpenCV 3 系列 。 


由 于 OpenCV 工 程 比较 大 ， 就 不 放 在 本 书 的 3rdparty 下 了 。 请 读者 从 
http:/Wopencv.org/downloads.html 下 载 ， 选 择 OpenCYV for Linux 版 本 即 可 。 
你 会 获得 一 个 像 opencv-3.1.0.zip 这 样 的 压缩 包 。 将 它 解 压 到 任意 目录 下 ， 
我 们 发 现 OpenCV 亦 是 一 个 cmake 工 程 。 


在 编译 之 前 ， 先 来 安装 OpenCV 的 依赖 项 : 


1 | sudo apt-get install build-essential libgtk2.0-dev libvtk5-dev libjpeg-dev libtiff4 
-dev libjasper-dev libopenexr-dev libtbb-dev 


事实 上 ，OpenCV 的 依赖 项 很 多 ， 缺 少 某 些 编译 项 会 影响 它 的 部 分 功 
能 〈 不 过 我 们 也 不 会 用 到 所 有 功能 ) 。OpenCV 会 在 cmake 阶 段 检 查 依 赖 
项 是 否 会 安装 ， 并 调整 自己 的 功能 。 如 果 你 的 电脑 上 有 GPU 并 且 安 装 了 
相关 依赖 项 ，OpenCV 也 会 把 GPU 加 速 打开 。 不 过 对 于 本 书 ， 上 面 那些 依 
赖 项 就 足够 了 。 


随后 的 编译 安装 和 普通 的 cmake 工 程 一 样 ， 请 在 make 之 后 ， 调 用 sudo 
make install 将 OpenCV 安 装 到 你 的 机 器 上 (而 不 是 仅 仪 编译 它 ) 。 视 机 器 
配置 ， 这 个 编译 过 程 大概 需 要 二 十 分 钟 到 一 个 小 时 不 等 。 如 果 你 的 CPU 
比较 强 ， 可 以 使 用 “make-j4” 这 样 的 命令 ， 调 用 多 个 线程 进行 编译 (-j 后 面 
的 参数 就 是 使 用 的 线程 数量 ) 。 安 装 后 ，OpenCV 默 认 存 储 在 /usr/local 目 
录 下 。 你 可 以 去 寻找 OpenCV 头 文件 与 库 文件 的 安装 位 置 ， 看 看 它们 都 在 
哪里 。 另 外 ， 如 果 之 前 已 经 安装 了 OpenCV 2 系列 ， 那 么 建议 你 把 OpenCV 
3 安装 到 别 的 地 方 ( 想 想 这 应 该 如 何 操作 ) o 


5.3.2 ”操作 OpenCV 图 像 


接 下 来 通过 一 个 例 程 熟 悉 一 下 OpenCV 对 图 像 的 操作 。 
四 slambook/ch5/imageBasics.cpp 


#include <iostream> 


#include <chrono> 


using namespace std; 


#include <opencv2/core/core.hpp> 


#include <opencv2/highgui/highgui .hpp> 


int main ( int argc, char** argv ) 
{ 
// 读 取 argv[1] 指定 的 图 像 
cv::Mat image; 
image = cv::imread ( argv[1] ); // cu::imread Ñ kik Kii ZK AATF HAR 
// 判断 图 像 文件 是 否 正确 读 取 
if ( image.data == nullptr ) // 数据 不 存在 ， 可 能 是 文件 不 存在 
cerr<<" 文 件 "<<argv[1]<<" 不 存在 ."<<endl; 


return 0; 


// 文件 顺利 读 取 ， 首 先 输出 一 些 基本 信息 
cout<<" 图 像 宽 为 "<<image.cols<<" ,高 为 "<<image.rows<<" ,通道 数 为 "<<image.channels 
()<<endl; 
cv::imshow ( "image", image ); // 用 cv::imshow 显示 图 像 
cv::waitKey ( 0 ); // 暂停 程序 ， 等 待 一 个 按键 输入 
// 判断 image 的 类 型 
if ( image.type() != CV_8UC1 && image.type() != CV_8UC3 ) 
{ 
// 图 像 类 型 不 符合 要 求 
cout<<" 请 输入 一 张 彩色 图 或 灰 度 图 ."<<endl; 
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return 0; 


// 遍历 图 像 ， 请 注意 以 下 遍历 方式 亦 可 使 用 于 随机 访问 
// 使 用 std::chrono 来 给 算法 计时 
chrono::steady_clock::time_point tl = chrono::steady_clock::now(); 


for ( size_t y=0; y<image.rows; y++ ) 


{ 
for ( size_t x=0; x<image.cols; x++ ) 
{ 
// 访问 位 于 z,y 处 的 像素 
// 用 cu::Mat::ptr 获得 图 像 的 行 指针 
unsigned char* row_ptr = image.ptr<unsigned char> ( y ); // row_ptr 是 
第 y 行 的 头 指针 
unsigned char* data_ptr = &row_ptr[ x*image.channels() ]; // data_ptr 
指向 待 访问 的 像素 数据 
// 输出 该 像素 的 每 个 通道 ， 如 果 是 灰 度 图 就 只 有 一 个 通道 
for ( int c = 0; c != image.channels(); c++ ) 
{ 
unsigned char data = data_ptr[c]; // data 为 I(a,y) 第 c 个 通道 的 值 
} 
} 
} 


chrono::steady_clock::time_point t2 = chrono::steady_clock: :now(); 
chrono::duration<double> time_used = chrono: :duration_cast<chrono: :duration< 
double>>( t2-t1 ); 

cout<<"i A A4% M at: "<<time_used.count()<<" #7, "<<endl; 


// 关于 cv::Mat HH MN 

// 直接 赋值 并 不 会 复制 数据 

cv::Mat image_another = image; 

// 修改 image_another 会 导致 image 发 生变 化 

image_another ( cv::Rect ( 0,0,100,100 ) ).setTo (0); // 将 左上 角 100x100 的 
KEE 

cv::imshow ( "image", image ); 


cv::waitKey ( 0 ); 


// 使 用 clone 函数 来 复制 数据 

cv::Mat image_clone = image.clone(); 

image_clone ( cv::Rect ( 0,0,100,100 ) ).setTo ( 255 ); 
cv::imshow ( "image", image ); 

cv::imshow ( "“image_clone", image_clone ); 


cv::waitKey ( 0 ); 


// 其 他 图 像 操 作 请 参见 0penCV 官方 文档 ， 查 询 每 个 函数 的 调用 方法 


70 cv: :destroyAllWindows () ; 
71 return 0; 


72 |} 


在 该 例 程 中 ， 我 们 演示 了 如 下 几 个 操作 : ARIZ. BM. RAI 
历 、 复 制 、 赋 值 等 。 大 部 分 的 注解 已 写 在 代码 里 面 。 编 译 该 程序 时 ， 你 
需要 在 CMakeLists.txt 中 添加 OpenCV 的 头 文件 ， 然 后 把 程序 链接 到 库 文件 
上 。 同 时 ， 由 于 使 用 了 C++11 标 准 (如 nullptr 和 chrono) ， 还 需要 设置 一 
下 编译 器 : 


1 |# 添加 C++ 11 标准 支持 
2 | set( CMAKE_CXX_FLAGS "-std=c++11" ) 


4 |# 寻找 0penCV Æ 

5 |find_package( OpenCV REQUIRED ) 

6 |# 添加 头 文件 

7 | include_directories( ${OpenCV_INCLUDE_DIRS} ) 
8 

9 


add_executable( imageBasics imageBasics.cpp ) 
10 | # 链接 OpenCV Æ 
11 | target_link_libraries( imageBasics ${OpenCV_LIBS} ) 


天 于 代码 ， 我 们 给 出 几 点 说 明 : 


1. 程 序 从 argv[1]， 也 就 是 命令 行 的 第 一 个 参数 中 读 取 图 像 位 置 。 我 们 
为 读者 准备 了 一 张 图 像 (ubuntu.png， 一 张 Ubuntu 的 壁纸 ， 和 希望 你 喜欢 ) 
供 测 试 使 用 。 因 此 ， 编 译 之 后 ， 使 用 如 下 命令 调用 此 程序 : 如 果 在 
Kdevelop 中 调用 此 程序 ， 请 务必 确保 把 参数 同时 给 它 。 这 可 以 在 启动 项 
中 配置 。 


1 | build/imageBasics ubuntu.png 


2. 程 序 的 10 人 17 行 ， 使 用 cv::imread 国 数 读 取 图 像 ， 并 把 图 像 和 基本 
信息 显示 出 来 。 

3. 在 32~~52 行 ， 遍 历 了 图 像 中 的 所 有 像素 ， 并 计算 了 整个 循环 所 用 的 
时 间 。 请 注意 像素 的 遍历 方式 并 不 是 唯一 的 ， 而 且 例 程 给 出 的 方式 也 不 


是 最 高 效 的 。OpenCV 提 供 了 和 迭代 器 ， 你 可 以 通过 迭代 器 损 历 图 像 的 像 
素 。 或 者 ，cv::Mat::data 提 供 了 指向 图 像 数 据 开 头 的 指针 ， 你 可 以 直接 通 
过 该 指针 自行 计算 偏 移 量 ， 然 后 得 到 像素 的 实际 内 存 位置 。 例 程 所 用 的 
方式 是 为 了 便于 读者 理解 图 像 的 结构 。 在 笔者 的 机 器 上 (虚拟 机 ) ， 遍 
历 这 张 图 像 用 时 大 约 12.74ms。 你 可 以 对 比 一 下 自己 机 器 上 的 速度 。 不 

过 ， 我 们 使 用 的 是 cmake 默 认 的 debug 模 式 ， ee a 


多 。 


4.0penCV 提 供 了 许多 对 图 像 进行 操作 的 水 数 ， 我 们 在 此 不 一 一 列 
举 ， 否 则 本 书 就 会 变 成 OpenCV 操 作 手 册 了 。 例 程 给 出 了 较为 常见 的 读 
取 、 显 示 操 作 ， 以 及 复制 图 像 中 可 能 陷入 的 深 拷 贝 误区 。 在 编程 过 程 
中 ， 读 者 还 会 磁 到 图 像 的 旋转 、 插 值 等 操作 ， 这 时 你 应 该 自行 查阅 水 数 
对 应 的 文档 ， 以 了 解 它 们 的 原理 与 使 用 方式 。 


应 该 指出 ， pe 它 只 是 许多 图 像 库 里 使 用 
汇 围 较 广 泛 的 一 个 。 不 过 ， 多 效 图 像 库 对 图 像 的 表达 是 大 同 小 异 的 。 我 
们 希望 读者 了 解 了 OpbenCV 对 图 像 的 表示 后 ; 能 够 理解 其 他 库 中 图 像 的 表 
达 ， 从 而 在 需要 数据 格式 时 能 够 自己 处 理 。 


另外 ， 由 于 cv::Mat 亦 是 矩阵 类 ， 除 了 表示 图 像 之 外 ， 我 们 也 可 以 用 
它 来 存储 位 姿 等 矩阵 数据 。 只 是 一 般 认 为 ，Eigen 对 于 固定 大 小 的 矩阵 使 
用 起 来 效率 更 高 一 些 。 


5.4 XBR: 拼接 点 云 


最 后 ， 我 们 来 练习 一 下 相机 内 外 参 的 使 用 方法 。 本 节 程 序 提供 了 5 张 
RGB-D 图 像 ， 并 且 知 道 每 个 图 像 的 内 参 和 外 参 。 根 据 RGB-D 图 像 和 相机 
内 参 ， 我 们 可 以 计算 任何 一 个 像素 在 相机 坐标 系 下 的 位 置 。 同 时 ， 根 所 
相机 位 姿 ， 又 能 计算 这 些 像素 在 世界 坐标 系 下 的 位 置 。 如 果 把 所 有 像素 
的 空间 坐标 都 求 出 来 ， 相 当 于 构建 一 张 类 似 于 地 图 的 东西 。 现 在 我 们 就 
来 练习 一 下 。 


我 们 准备 了 5 对 图 像 ， 位 于 slambook/ch5/joinMap 中 。 在 color/ 下 有 
1.png 到 5.png 共 5 张 RGB 图 ， 而 在 depth/ 下 有 5 张 对 应 的 深度 图 。 同 时 ， 
pose.txt 文 件 给 出 了 5 张 图 像 的 相机 位 姿 (LAT, 形式 ) 。 位 姿 记录 的 形式 


是 平移 向 量 加 旋转 四 元 数 : 


[2 
其 中 gq, 是 四 元 数 的 实 部 。 例 如 ， 第 一 对 图 的 外 参 为 : 
[一 0.228993, 0.00645704, 0.0287837, —0.0004327, —0.113131, —0.0326832, 0.993042]. 


下 面 我 们 写 一 段 程序 ， 完 成 两 件 事 : (1). 根 据 内 参 计算 一 对 RGB-D 图 
像 对 应 的 点 云 ; (2). 根 据 各 张 图 的 相机 位 姿 (也 就 是 外 参 ) ， 把 点 云 加 起 
来 ， 组 成 地 图 。 


本 书 的 点 云 库 使 用 PCL (Point Cloud Library) o PCL ZÆ LLRA 


易 ， 执 行 以 下 命令 即 可 外 : 安装 完成 后 ，PCL 的 头 文件 将 安装 
在 /usrinclude/pcl-1.7 下 ， 库 文件 位 于 /usvlib/ 下 。 


1 | sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl 
2 | sudo apt-get update 
3 | sudo apt-get install libpcl-all 


现在 编写 拼接 部 分 的 程序 : 
内 slambook/ch5/joinMap/joinMap.cpp 


#include <iostream> 


#include <fstream> 


using namespace std; 


#include <opencv2/core/core.hpp> 


#include <opencv2/highgui/highgui .hpp> 


#include <Eigen/Geometry> 


#include <boost/format.hpp> // for formating strings 


#include <pcl/point_types.h> 
#include <pcl/io/pcd_io.h> 


#include <pcl/visualization/pcl_visualizer.h> 


int main( int argc, char** argv ) 


{ 


vector<cv::Mat> colorImgs, depthImgs; // 彩色 图 和 深度 图 
Vector<Eigen::Isometry3d> poses; // 相机 位 姿 


ifstream fin("./pose.txt"); 
if (!fin) 
{ 
cerr<<" 请 在 有 pose.txt 的 目录 下 运行 此 程序 "<<endl; 


return 1; 


for ( int i=0; i<5; i++ ) 


{ 
boost::format fmt("./%s/%d.%s" ); // 图 像 文件 格式 
colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str(Q) )); 
depthImgs.push_back( cv::imread( (fmt/"depth"%(i+1)%"pgm").str(), -1 )); // 
使 用 -1 读 取 原始 图 像 
double data[7] = {0}; 
for ( auto& d:data ) 
fin>>d; 
Eigen::Quaterniond q( data[6], data[3], data[4], data[5] ); 
Eigen: :Isometry3d T(q); 
T.pretranslate( Eigen::Vector3d( data[0], data[i], data[2] )); 
poses.push_back( T ); 
} 


// 计算 点 云 并 拼接 
// 相机 内 参 
double cx = 325.5; 


double cy 
double fx 
double fy 


253.5; 
518.0; 
519.0; 


double depthScale = 1000.0; 


cout<<" 正 在 将 图 像 转 换 为 点 云 ..."<<endl; 

// 定义 点 云 使 用 的 格式 : 这 里 用 的 是 XYZRGB 
typedef pcl::PointXYZRGB PointT; 

typedef pcl::PointCloud<PointT> PointCloud; 


// 新 建 一 个 点 云 
PointCloud::Ptr pointCloud( new PointCloud ); 


for ( int i=0; i<5; i++ ) 


{ 


cout<<" 转换 图 像 中 : "<<i+1<<endl; 
cv::Mat color = colorImgs[i]; 


cv::Mat depth = depthImgs [i]; 


Eigen::Isometry3d T = poses[i]; 


for ( int v=0; v<color.rows; v++ ) 


for ( int u=0; u<color.cols; ut+ ) 


{ 


unsigned int d = depth.ptr<unsigned short> ( v )[ul; // 深度 值 
if ( d==0 ) continue; // 为 0 表示 没有 测量 到 


Eigen::Vector3d point; 

point[2] = double(d)/depthScale; 
point [0] (u-cx) *point [2] /fx; 

point [1] = (v-cy)*point [2] /fy; 
Eigen::Vector3d pointWorld = T*point; 


PointT p ; 
p.x = pointWorld[0]; 
p-y = pointWorld[1]; 
pointWorld [2]; 
color.data[ v*color.steptu*color.channels() ]; 
color.data[ v*color.steptu*color.channels()+1 ]; 
x color.data[ v*color.stept+tu*color.channels()+2 ]; 


pointCloud->points.push_back( p ); 


pointCloud->is_dense = false; 
cout<<" 4, & # A "<<pointCloud->size()<<"4 & ."<<end1; 


pcl::io::savePCDFileBinary("map.pcd", *pointCloud ); 


return 0; 


一 点 说 明 : 

1.14~37 行 ， 读 取 彩 色 和 深度 图 像 对 及 位 姿 信 息 ， 并 把 位 姿 从 四 元 数 
与 平移 向 量 转换 为 变换 矩阵 。 注 意 程序 里 使 用 了 boost::format 进 行 字 符 串 
的 格式 化 。 


2.65~78 行 : HA MF Uv) REAd 的 像素 在 相机 坐标 系 下 的 位 
置 ， 并 根据 外 参 把 它们 变换 到 世界 坐标 。 我 们 知道 ， 相 机 坐标 p. 到 像素 


坐标 (u,v,d ) 的 关系 为 


d VU 一 Kp. (5.17) 


反 推 p. 的 形式 亦 非 常 简单 。 设 p, =[xyz ]， 那 么 : 
z=d 
y 一 fy 


3. 为 了 编译 此 程序 ， 我 们 需 3 个 库 : Eijgen、OpenCV 和 PCL。 因 此 主 
程序 的 CMake-Lists.txt 应 该 如 下 : 


1 | # opencV 
2 | find_package( OpenCV REQUIRED ) 
3 | include_directories( ${OpenCV_INCLUDE_DIRS} ) 


5 |# eigen 


6 | include_directories( "/usr/include/eigen3/" ) 


8 |# pcl 

9 |find_package( PCL REQUIRED COMPONENT common io ) 
10 | include_directories( ${PCL_INCLUDE_DIRS} ) 

1 | add_definitions( ${PCL_DEFINITIONS} ) 


13 | add_executable( joinMap joinMap.cpp ) 
14 | target_link_libraries( joinMap ${OpenCV_LIBS} ${PCL_LIBRARIES} ) 


最 后 ， 我 们 把 生成 的 点 云 以 PCD 格 式 存 储 在 map.pcd 中 。 用 PCL 提 供 
的 可 视 化 程序 打开 这 个 文件 : 


1 |pcl_viewer map.pcd 


随后 就 可 以 看 到 拼合 的 点 云 地 图 了 ( 见 图 5-9) 。 你 可 以 拖 动 鼠 标 查 


看 。 


REPRE 


OO: Srat 
图 5-9 ”查看 拼合 的 点 云 地 图 。 


在 这 个 例 程 中 ， 我 们 使 用 相机 内 参 和 外 参 来 计算 像素 在 世界 坐标 系 
中 的 位 置 ， 并 把 它们 合并 成 一 个 点 云 。 这 是 一 个 综合 性 的 示例 ， 请 读者 
仔细 体会 并 掌握 其 内 容 。 

习题 

1.* 寻 找 一 部 相机 〈 你 的 手机 或 笔记 本 的 摄像 头 即 可 ) ， 标 定 它 的 内 
参 。 你 可 能 会 用 到 标定 板 ， 或 者 自己 打印 一 张 标 定 用 的 棋盘 格 。 

2 .叙述 相机 内 参 的 物理 意义 。 如 果 一 部 相机 的 分 辨 率 变 为 原来 的 两 倍 
而 其 他 地 方 不 变 ， 它 的 内 参 如 何 变化 ? 

3. 搜 索 特 殊 相机 ( 鱼 眼 或 全 景 相机 ) 的 标定 方法 。 它 们 与 普通 的 针 孔 
模型 有 何不 同 

4. 调 研 全 局 快门 相机 (global shutter) 和 卷 帘 快 门 相 机 (rolling 
shutter) 的 异同 。 它 们 在 SLAM 中 有 何 优 缺 点 ? 

5.RGB-D 相 机 是 如 何 标定 的 ? 以 Kinect 为 例 ， 需 要 标定 哪些 参数 ? 

(参照 https:/github.comy/code-iaiyiai_kinect2。 ) 

6. 除 了 示例 程序 演示 的 遍历 图 像 的 方式 ， 你 还 能 举 出 哪些 遍历 图 像 的 
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法 : 


7.* 阅 读 OpenCV 官 方 教程 ， 学 习 它 的 基本 用 法 。 


[1] 或 图 像 坐标 系 ， 见 本 讲 第 2 节 。 
[2] 是 的 ， 它 不 再 直 了 ， 而 是 变 成 弯 的 。 如 果 往 里 弯 ， 称 为 桶 形 失真 ; 往外 弯 则 是 枕 形 失真 。 


[3] 注意 到 Zz 可 能 小 于 1， 说 明 该 点 位 于 归 一 化 平面 后 面 ， 它 可 能 不 会 在 相机 平面 上 成 像 ， 实 践 当 
中 要 检查 一 次 。 


[4] 那样 的 话 外 观 会 有 些 奇特 。 

[5] 读者 可 以 自己 用 眼睛 模拟 一 下 。 
[6] 官方 主页 : http:/opencv.org。 
[7] 官网 : http://pointclouds.org/o 


[8] 在 Ubuntu 16.04 下 请 通过 apt-get 安 装 ， 想 想 这 该 怎么 做 。 


第 6 讲 ” 非 线性 优化 


主要 目标 
1. 理 解 最 小 二 乘法 的 含义 和 处 理 方式 。 


2. 理 解 高 斯 牛顿 法 (Gauss-Newton) 、 列 文 伯 格 一 马 夸 尔 特 方法 
(Levenburg-Marquadt) 等 下 降 策略 。 


3. 学 习 Ceres 库 和 g2o 库 的 基本 使 用 方法 。 


在 前 面 几 讲 ， 我 们 介绍 了 经 典 SLAM 模 型 的 运动 方程 和 观测 方程 。 现 
在 我 们 已 经 知道 ， 方 程 中 的 位 姿 可 以 由 变换 和 矩阵 来 描述 ， 然 后 用 李 代 数 
进行 优化 。 观 测 方 程 由 相机 成 像 模 型 给 出 ， 其 中 内 参 是 随 相 机 固定 的 ， 
而 外 参 则 是 相机 的 位 次 。 于 是 ， 我 们 已 经 弄 清 了 经 典 SLAM 模 型 在 视觉 情 
况 下 的 具体 表达 。 


然而 ， 由 于 噪声 的 存在 ， 运 动 方 程 和 观测 方程 的 等 式 必定 不 是 精确 
成 立 的 。 尽 管 相 机 可 以 非常 好 地 符合 针 孔 模型 ， 但 遗憾 的 是 ， 我 们 得 到 
的 数据 通常 是 受 各 种 未 知 噪声 影响 的 。 即 使 我 们 有 高 精度 的 相机 ， 运 动 
方程 和 观测 方程 也 只 能 近似 成 立 。 所 以 ， 与 其 假设 数据 必须 符合 方程 ， 
不 如 来 讨论 如 何在 有 品 声 的 数据 中 进行 准确 的 状态 估计 。 


大 多 效 现代 视觉 SLAM 算 法 都 不 需要 那么 高 成 本 的 传感器 ， 甚 至 也 不 
需要 那么 昂贵 的 处 理 器 来 计算 这 些 数据 ， 这 全 是 算法 的 功 务 。 由 于 在 
SLAM 问 题 中 ， 同 一 个 点 往往 会 被 一 部 相机 在 不 同 的 时 间 内 多 次 观测 ， 同 
一 部 相机 在 每 个 时 刻 观 测 到 的 点 也 不 止 一 个 。 这 些 因 素 交 织 在 一 起 ， 使 
我 们 拥有 了 更 多 的 约束 ， 最 终 能 够 较 好 地 从 噪声 数据 中 恢复 出 我 们 需要 
的 东西 。 本 节 就 将 介绍 如 何 通过 优化 处 理 噪声 数据 ， 并 且 由 这 些 表层 逐 
渐 深 入 到 图 优化 本 质 ， 给 出 图 优化 的 解决 算法 初步 介绍 并 且 提供 训练 实 
例 。 


Fonas ~ Feo 4 Of (ax 


+ A ax! H (x) ax 
2 


Lory 


6.1 ”状态 估计 问题 
6.1.1 最 大 后 验 与 最 大 似 然 


接着 前 面 几 讲 的 内 容 ， 我 们 回顾 一 下 第 2 讲 讨论 的 经 典 SLAM 模 型 。 
它 由 一 个 状态 方程 和 一 个 运动 方程 构成 ， 如 式 (2.5) 所 示 : 


(6.1) 
Zk j = h (Yj, £k) + Vk,j 


| te= f (Tk-1, Uk) + Wk 


通过 第 4 讲 的 知识 ， 我 们 了 解 到 这 里 的 x 乃 是 相机 的 位 姿 。 我 们 可 以 
使 用 变换 矩阵 或 李 代 数 表 示 它 。 至 于 观测 方程 ， 第 5 讲 已 经 说 明 ， 即 针 孔 
相机 模型 。 为 了 让 读者 对 它们 有 更 深 的 印象 ， 我 们 不 妨 讨论 一 下 其 具体 
参数 化 形式 。 首 先 ， 位 姿 变 量 x 可 以 由 T Rept) RA, CEREM 
的 。 由 于 运动 方程 在 视觉 SLAM 中 没有 特殊 性 ， 我 们 暂且 不 讨论 ， 而 主要 
讨论 观测 方程 。 假 设 在 x, 处 对 路 标 y 进行 了 一 次 观测 ， 对 应 到 图 像 上 的 像 
素 位 置 7, ， 那 么 ， 观 测 方程 可 以 表示 成 


szk j = K exp (£^) yy. (6.2) 


根据 上 一 讲 的 内 容 ， 读 者 应 该 知道 ， 这 里 K 为 相机 内 参 ，s 为 像素 点 
的 距离 。 同 时 ， 这 里 的 z 和 y 都 必须 以 齐 次 坐标 来 描述 ， 且 中 间 有 一 次 


齐 次 到 非 齐 次 的 转换 。 如 果 你 还 不 熟悉 这 个 过 程 ， 请 回 到 上 一 讲 再 仔细 
阅读 。 


现在 ， 考 虑 数据 受 噪 声 影 响 后 会 发 生 什 么 改变 。 在 运动 和 观测 方程 
中 ， 我 们 通常 假设 两 个 噪声 项 w, ,v， 满足 零 均值 的 高 斯 分 布 : 


wp ~ N (0, Re), vk ~ N (0, Qk,;). (6.3) 


在 这 些 品 声 的 影响 下 ， 我 们 希望 通过 市 噪声 的 数据 z 和 wu HET ex 
和 地 图 y (以 及 它们 的 概率 分 布 ) ， 这 构成 了 一 个 状态 估计 问题 。 由 于 在 


SLAM 过 程 中 ， 这 些 数据 是 随时 间 逐 渐 到 来 的 ， 所 以 在 历史 上 很 长 一 段 时 
间 内 ， 研 究 者 们 使 用 滤波 器 ， 尤 其 是 扩展 卡尔 曼 滤 波 器 (EKF) 求解 
它 。 卡 尔 曼 滤波 器 关心 当前 时 刻 的 状态 估计 x, ， 而 对 之 前 的 状态 则 不 多 
考虑 ;相对 地 ， 近 年 来 普遍 使 用 的 非 线 性 优化 方法 ， 使 用 所 有 时 刻 来 集 
到 的 数据 进行 状态 估计 ， 被 认为 优 于 传统 的 滤波 器 (3 ， 而 成 为 当前 视觉 
SLAM 的 主流 方法 。 因 此 ， 本 书 重 点 介绍 以 非 线性 优化 为 主 的 优化 方法 ， 
对 卡尔 曼 滤 疲 器 则 留 到 第 10 讲 再 进行 讨论 。 本 讲 将 介绍 非 线 性 优化 的 基 
本 知识 ， 然 后 在 第 10 讲 、 第 11 讲 中 对 它们 进行 更 深入 的 分 析 。 


首先 ， 从 概率 学 角度 来 看 一 下 我 们 正在 讨论 什么 问题 。 在 非 线 性 优 
化 中 ， 把 所 有 待 估计 的 变量 放 在 一 个 “状态 变量 ”中 : 


现在 ,我 们 说 ， 对 机 器 人 状态 的 估计 ， 束 是 已 知 输入 数据 u 和 观测 数 
据 z 的 条 件 下 ， 求 计算 状态 x 的 条 件 概率 分 布 : 


P(x|z, u). (6.4) 


类 似 于 x ， 这 里 u 和 z 也 是 对 所 有 数据 的 统称 。 特 别 地 ， 当 没有 测量 
运动 的 传感器 ， 而 只 有 一 张 张 的 图 像 时 ， 即 只 考虑 观测 方程 带 来 的 数据 
时 ， 相 当 于 估计 P (xlz ) 的 条 件 概 率 分 布 。 如 果 忽 略图 像 在 时 间 上 的 联 
系 ， 把 它们 看 作 一 堆 彼 此 没有 关系 的 图 片 ， 该 问题 也 称 为 Structure from 
Motion (SfM) ， 即 如 何 从 许多 图 像 中 重建 三 维 空间 结构 己 。 在 这 种 情 
况 下 ，SLAM 可 以 看 作 图 像 具 有 时 间 先 后 顺序 ， 需 要 实时 求解 一 个 SfM 问 
题 。 为 了 估计 状态 变量 的 条 件 分 布 ， 利 用 贝 叶 斯 法 则 ， 有 : 


P (z|æ) P (a) 


P (22) = — pr 


x P (z|æ) P (x). (6.5) 


贝 叶 斯 法 则 左 侧 通常 称 为 后 验 概率 ， 右 侧 的 P (zlx ) 称 为 似 然 ， 另 一 
部 分 P (x ) 称 为 先 验 。 直 接 求 后 验 分 布 是 困难 的 ， 但 是 求 一 个 状态 最 优 估 
计 ， 使 得 在 该 状态 下 后 验 概率 最 大 化 (Maximize a Posterior, MAP) ， 
则 是 可 行 的 : 


x* map = argmax P (2|z) = arg max P(z|ax) P(x). (6.6) 


请 注意 贝 叶 斯 法 则 的 分 母 部 分 与 待 估计 的 状态 x 无 和 天 ， 因 而 可 以 忽 
略 。 贝 叶 斯 法 则 告诉 我 们 ， 求 解 最 大 后 验 概率 相当 于 最 大 化 似 然 和 先 验 
的 乘积 。 进 一 步 ， 我 们 当然 也 可 以 说 ， 对 不 起 ， 我 不 知道 机 器 人 位 姿 大 
概 在 什么 地 方 ， 此 时 就 没有 了 先 验 。 那 么 ， 可 以 求解 x 的 最 大 似 然 估计 
( Maximize Likelihood Estimation, MLE) : 


TMLE = arg max P(z|z). (6.7) 


直观 讲 ， 似 然 是 指 “ 在 现在 的 位 姿 下 ， 可 能 产生 怎样 的 观测 数据 ”。 
由 于 我 们 知道 观测 数据 ， 所 以 最 大 似 然 估 计 可 以 理解 成 :“ 在 什么 样 的 状 
态 下 ， 最 可 能 产生 现在 观测 到 的 数据 * 。 这 就 是 最 大 似 然 估 计 的 直观 意 
义 。 


6.1.2 ”最 小 二 乘 的 引出 

那么 如 何 求 最 大 似 然 估计 呢 ? 我 们 说 ， 在 高 斯 分 布 的 假设 下 ， 最 大 
似 然 能 够 有 较 简 单 的 形式 。 回 顾 观 测 模型 ， 对 于 某 一 次 观测 : 

Zk j = h (Yj, £k) + Vk,j» 
由 于 我 们 假设 了 噪 再 项 w ~N (0,Q, )， 所 以 观测 数据 的 条 件 概 率 为 
P(zik|Ek, Yi) = N (h(Yj, £k), Qk,j). 

它 依然 是 一 个 高 斯 分 布 。 为 了 计算 使 它 最 大 化 的 x, ,y, ， 我 们 往往 使 

用 最 小 化 负 对 数 的 方式 来 求 一 个 高 斯 分 布 的 最 大 似 然 。 


高 斯 分 布 在 负 对 效 下 有 较 好 的 效 学 形式 。 考 虑 任意 高 维 高 斯 分 布 x 人 ~ 
N( 2， 它 的 概率 密度 函数 展开 形式 为 


1 1 Thi 
P (x) = exp | —=>(x- u) X` (a@—p)}. (6.8) 
(an) det(£) pf 2 m) 


对 其 取 负 对 数 ， 则 变 为 


-In (P (æ)) = $ In ((27)" det (32)) + $ (æ — 4)" (æ — n). (6.9) 


对 原 分 布 求 最 大 化 相当 于 对 负 对 数 求 最 小 化 。 在 最 小 化 上 式 的 x 时 ， 
第 一 项 与 x 无 天 ， 可 以 略 去 。 于 是 ， 只 要 最 小 化 右 侧 的 二 次 型 项 ， 融 得 到 
了 对 状态 的 最 大 似 然 估计 。 代 入 SLAM 的 观测 模型 ， 相 当 于 在 求 : 


Z = arg min (Ci —h (x, ¥j)) QE (Zkj—h (ær, y3))) : (6.10) 


我 们 发 现 ， 该 式 等 价 于 最 小 化 噪声 项 〈《 即 误差 ) 的 平方 (5 范 数 意义 
F) 。 因 此 ， 对 于 所 有 的 运动 和 任意 的 观测 ， 我 们 定义 数据 与 估计 值 之 
间 的 误差 : 


Evk = Lk — f (Ek-1, Uk) (6.11) 


ey jk = Zkj — h (eK, Yj), 


J(x) = DD el „R; evk + ` ` er Qk nai: (6.12) 
k k j 


并 求 该 误差 的 平方 和 : 这 样 就 得 到 了 一 个 总 体 意 义 下 的 最 小 二 乘 问 
题 (Least Square Problem) 。 我 们 明白 它 的 最 优 解 等 价 于 状态 的 最 大 似 然 
估计 。 直 观 讲 ， 由 于 噪声 的 存在 ， 当 我 们 把 估计 的 轨迹 与 地 图 代入 SLAM 
的 运动 、 观 测 方程 中 时 ， 它 们 并 不 会 完美 地 成 立 。 这 时 怎么 办 呢 ? 我 们 
对 状态 的 估计 值 进 行 微 调 ， 使 得 整体 的 误差 下 降 一 些 。 当 然 这 个 下 降 也 
有 限度 ， 它 一 般 会 到 达 一 个 极 小 值 。 这 就 是 一 个 典型 非 线性 优化 的 过 
程 。 

仔细 观察 式 (6.12)， 我 们 发 现 SLAM 中 的 最 小 二 乘 问题 具有 一 些 特定 
的 结构 : 

首先， 整个 问题 的 目标 函数 由 许多 个 误差 的 (加权 的 ) 平方 和 组 
成 。 虽 然 总 体 的 状态 变量 维 数 很 高 ， 但 每 个 误差 项 都 是 简单 的 ， 仪 与 一 
两 个 状态 变量 有 关 。 例 如 ， 运 动 误差 只 与 % ,x, 有 关 ， 观 测 误差 只 与 x, ;y, 
有 关 。 每 个 误差 项 都 是 一 个 小 规模 的 约束 ， 我 们 之 后 会 谈论 如 何 对 它们 


进行 线性 近似 ， 最 后 再 把 这 个 误差 项 的 小 雅 可 比 窍 阵 块 放 到 整体 的 雅 可 
比 矩 阵 中 。 由 于 这 种 做 法 ,我 们 称 每 个 误差 项 对 应 的 优化 变量 为 参数 块 


( Parameter Block) 。 


.整体 误差 由 很 多 小 型 误差 项 之 和 组 成 的 问题 ， 其 增 量 方程 的 求解 会 
具有 一 定 的 稀疏 性 (会 在 第 10 讲 详细 讲解 ) ， 使 得 它们 在 大 规模 时 亦 可 
求解 。 

其 次 ， 如 果 使 用 李 代 数 表 示 ， 则 该 问题 是 无 约束 的 最 小 二 乘 问题 。 
但 如 果 用 旋转 矩阵 《变换 矩阵 ) 描述 位 姿 ， 则 会 引入 旋转 和 矩 阵 自身 的 约 
束 〈 旋 转 和 矩阵 必须 是 正 交 和 矩阵 且 行 列 式 为 1) 。 人 额外 的 约束 会 使 优化 变 得 
更 困难 。 这 体现 了 李 代 数 的 优势 。 

最后， 我 们 使 用 了 平方 形式 (二 范 数 ) 度量 误差 ， 它 是 直观 的 ， 相 
当 于 欧 氏 空间 中 距离 的 平方 。 但 也 存在 着 一 些 问 题 ， 并 且 不 是 唯一 的 度 
量 方式 。 我 们 亦 可 使 用 其 他 的 范 数 构建 优化 问题 。 

现在 ， 我 们 介绍 如 何 求解 这 个 最 小 二 乘 问 题 。 本 讲 将 介绍 非 线 性 优 
化 的 基本 知识 ， 特 别 地 ， 针 对 这 样 一 个 通用 的 无 约束 非 线 性 最 小 二 乘 问 
题 ， 探 讨 它 是 如 何 求解 的 。 在 后 续 几 讲 ， 我 们 会 大 量 使 用 本 讲 的 结果 ， 
详细 讨论 它 在 SLAM 前 端 、 后 端 中 的 应 用 。 


6.2 ” 非 线性 最 小 二 乘 


我 们 先 来 考虑 一 个 简单 的 最 小 二 乘 问题 : 
min 5 If («| (6.13) 


这 里 自 变量 xE R", f 是 任意 非 线 性 阅 数 ， 我 们 设 它 有 m 维 : f(x je 
R"。 下 面 讨 论 如 何 求解 这 样 一 个 优化 间 题 。 

如 果 f 是 个 数学 形式 上 很 简单 的 冰 数 ， 那 么 问题 也 许可 以 用 解析 形式 
来 求 。 令 目标 函数 的 导数 为 零 ， 然 后 求解 x 的 最 优 值 ， 融 和 求 二 元 函数 的 
极 值 一 样 : 


SF 


0. .14 
dz O24) 


解 此 方程 ， 就 得 到 了 导数 为 零 处 的 极 值 。 它 们 可 能 是 极 大 、 极 小 或 
鞍点 处 的 值 ， 只 要 逐个 比较 它们 的 函数 值 大 小 即 可 。 但 是 ， 这 个 方程 是 
否 容 易 求解 呢 ?这 取决 于 f 导 阅 数 的 形式 。 在 SLAM 中 ， 我 们 使 用 李 代 数 
来 表示 机 器 人 的 旋转 和 位 移 。 尽 管 我 们 在 李 代 数 一 讲 讨 论 了 它 的 导数 形 
式 ， 但 这 不 代表 我 们 就 能 够 顺利 求解 上 式 这 样 一 个 复杂 的 非 线 性 方程 。 

对 于 不 方便 直接 求解 的 最 小 二 乘 问 题 ， 我 们 可 以 用 迭代 的 方式 ， 从 
一 个 初始 值 出 发 ， 不 断 地 更 新 当前 的 优化 变量 ， 使 目标 函数 下 降 。 具 体 
步骤 可 列 写 如 下 : 


1. 给 定 某 个 初始 值 x  。 


2. 对 于 第 k 次 迭代 ， 寻 找 一 个 增 量 Ax, ， 使 得 /f(x +Ax, ) / ? ,达到 极 
小 值 。 
3. 若 Ax, 足够 小 ， 则 停止 。 


4. 否 则 2 Sx, +1 =X; +Ax,, 2 返回 第 2 步 。 


这 让 求解 导 函 数 为 零 的 问题 变 成 了 一 个 不 断 寻找 梯度 并 下 降 的 过 
程 。 直 到 某 个 时 刻 增 量 非常 小 ， 无 法 再 使 国 数 下 降 。 此 时 算法 收效 ， 目 
标 达到 了 一 个 极 小 ， 我 们 也 就 完成 了 寻找 极 小 值 的 过 程 。 在 这 个 过 程 
中 ， 我 们 只 要 找到 迭代 点 的 梯度 方向 即 可 ， 而 无 须 寻 找 全 局 导 阔 效 为 零 
的 情况 。 

接 下 来 的 问题 是 ， 增 量 Ax 如 何 确 定 ? 实际 上 ， 研 究 者 们 已 经 花费 了 
大 量 精力 探索 增 量 的 求解 方式 。 我 们 将 介绍 两 类 办 法 ， 用 不 同 的 手段 来 
寻找 这 个 增 量 。 目 前 这 两 种 方法 在 视觉 SLAM 的 优化 问题 上 被 广泛 采用 ， 
大 多 数 优化 库 都 可 以 使 用 它们 。 


6.2.1 ”一 阶 和 二 阶梯 度 法 
求解 增 量 最 直观 的 方式 是 将 目标 孙 数 在 x 附近 进行 泰勒 展开 : 


f(s + Ae)|B ~ Fæ)? + J (2) Az + Se" HAs. (6.15) 


这 里 J 是 /f(x)/? 关 于 x 的 导数 FEAT) ， 而 五 则 是 二 阶 导 
数 ( 海 塞 (Hessian) FORE) 。 我 们 可 以 选择 保留 泰勒 展开 的 一 阶 或 二 阶 
项 ， 对 应 的 求解 方法 则 为 一 阶梯 度 或 二 阶梯 度 法 。 如 果 保 留 一 阶梯 度 ， 
那么 增 量 的 解 就 为 

Aa* 一 一 .JI(z). (6.16) 

它 的 直观 意义 非常 简单 ， 只 要 我 们 沿 着 反 向 梯度 方向 前 进 即 可 。 通 
常 我 们 还 会 计算 该 方向 上 的 一 个 步 长 ， 求 得 最 快 的 下 降 方式 。 这 种 方法 
被 称 为 最 速 下 降 法 。 

另 一 方面 ， 如 果 保 留 二 阶梯 度 信息 ， 那 么 增 量 方程 为 


Ag* = argmin ||f (æ) ||} + J (x) Aw + 5 Ae" H Ac. (6.17) 


求 右 侧 等 式 关 于 Ax 的 导数 并 令 它 为 零 ， 就 得 到 了 增 量 的 解 : 
HAz = —J". (6.18) 


该 方法 又 称 为 牛顿 法 。 我 们 看 到 ， 一 阶 和 二 阶梯 度 法 都 十 分 直观 ， 
只 要 把 疯 数 在 迭代 点 附近 进行 泰勒 展开 ， 并 针对 更 新 量 做 最 小 化 即 可 。 
由 于 泰勒 展开 之 后 函数 变 成 了 多 项 式 ， 所 以 求解 增 量 时 只 需 解 线性 方程 
即 可 ， 避 免 了 直接 求 导 函数 为 零 这 样 的 非 线性 方程 的 困难 。 不 过 ， 这 两 
种 方法 也 存在 它们 自身 的 问题 。 最 速 下 降 法 过 于 贪心 ， 容 易 走 出 锯齿 路 
线 ， 反 而 增加 了 迭代 次 数 。 而 牛顿 法 则 需要 计算 目标 阔 效 的 五 号 阵 ， 这 
在 问题 规模 较 大 时 非常 困难 ， 我 们 通常 倾向 于 避免 卫 的 计算 。 所 以 ， 接 
下 来 我 们 详细 地 介绍 两 类 更 加 实用 的 方法 : 高 斯 牛顿 法 和 列 文 伯 格 一 马 
SRT Bo 


6.2.2 ”高 斯 牛顿 法 


高 斯 牛顿 法 是 最 优化 算法 中 最 简单 的 方法 之 一 。 它 的 思想 是 将 f (x ) 
进行 一 阶 的 泰勒 展开 〈 请 注意 不 是 目标 函数 fx)) : 


f(x + Arx) = f (x) + J (x) Ax. (6.19) 


这 里 J (x ) 为 f(x ) 关 于 x 的 导数 ， 实 际 上 是 一 个 mxn 和 矩阵 ， 也 是 一 个 雅 
可 比 和 矩阵 。 根 据 前 面 的 框架 ， 当 前 的 目标 是 寻找 下 降 矢 量 Ax ， 使 得 /f(x 
+Ax) /: 达到 最 小 。 为 了 求 Ax ， 我 们 需要 解 一 个 线性 的 最 小 二 乘 问题 : 


1 
Arz” = arg min 本 | 下 (x) + J (x) Ax’. (6.20) 


这 个 方程 与 之 前 有 什么 不 一 样 呢 ? RERBART, SEMA MMR 
对 Ax 求 导 ， 并 令 导 数 为 零 。 由 于 这 里 考虑 的 是 Ax 的 导数 (而 不 是 x ) ， 
我 们 最 后 将 得 到 一 个 线性 的 方程 。 为 此 ， 先 展开 目标 阔 数 的 平方 项 : 


5 (a) + J (æ) A = 5(f (x) + J (x) As)" (f (æ) + J (x) Az) 


1 
8 
5 (IŒI + 2F (w)" J(@)Aw + Aa I(a)"T(@) Az). 
求 上 了 式 关 于 Ax 的 导数 ， 并 令 其 为 零 : 
2I (a) f (x) +2I(x)' J (x) Ax = 0. 
可 以 得 到 如 下 方程 组 : 
J(x)' J (x) Ax = —J(ax)'f (x). (6.21) 


注意 ， 我 们 要 求解 的 变量 是 Ax ， 因 此 这 是 一 个 线性 方程 组 ， 我 们 称 
它 为 增 量 方程 ， 也 可 以 称 为 高 斯 牛顿 方程 (Gauss Newton equation) 或 
者 正规 方程 (Normal equation) 。 我 们 把 左边 的 系数 定义 为 ， 右 边 定 
义 为 9 ， 那 么 上 式 变 为 


五 Az =g. (6.22) 


这 里 把 左 侧记 作 瓦 是 有 意义 的 。 对 比 牛 顿 法 可 见 ， 高 斯 牛顿 法 用 JTJ 
作为 牛顿 法 中 二 阶 Hessian 和 矩阵 的 近似 ， 从 而 省 略 了 计算 五 的 过 程 。 求 解 
增 量 方程 是 整个 优化 问题 的 核心 所 在 。 如 果 我 们 能 够 顺利 解 出 该 方程 ， 
那么 高 斯 牛顿 法 的 算法 步骤 可 以 写成 : 

1. 给 定 初始 值 x 。。 


2. 对 于 第 k RIA, Kh SUB HEAT Ce ABBE (x, ) 和 误差 f (x )。 


3. 求 解 增 量 方程 : H Ax, =g o 
4.4 Ax, 足够 小 ， 则 停止 。 否 则 ， 令 x ,, =x, +Ax, ， 返 回 第 2 步 。 


从 算法 步骤 中 可 以 看 到 ， 增 量 方程 的 求解 占据 着 主要 地 位 。 原 则 
上 ， 它 要 求 我 们 所 用 的 近似 互 矩 阵 是 可 逆 的 〈 而 且 是 正定 的 ) ， 但 实际 
数据 中 计算 得 到 的 JIzJ 却 只 有 半 正 定性 。 也 就 是 说 ， 在 使 用 高 斯 牛顿 法 
时 ， 可 能 出 现 J7J 为 奇异 和 矩阵 或 者 病态 (illcondition) 的 情况 ， 此 时 增 量 
的 稳定 性 较 差 ， 导 致 算法 不 收敛。 更 严重 的 是 ， 就 算 我 们 假设 五 非 奇 异 
也 非 病 态 ， 如 果 我 们 求 出 来 的 步 长 Ax 太 大 ， 也 会 导致 我 们 采用 的 局 部 近 
似 (6.19) 不 够 准确 ， 这 样 一 来 我 们 甚至 无 法 保证 它 的 迭代 收敛 ， 哪 怕 是 让 
目标 函数 变 得 更 大 都 是 有 可 能 的 。 

尽管 高 斯 牛顿 法 有 这 些 缺 点 ， 但 它 依然 值 得 我 们 去 学 习 ， 因 为 在 非 
线性 优化 里 ， 相 当 多 的 算法 都 可 以 归结 为 高 斯 牛顿 法 的 变种 。 这 些 算 法 
都 借助 了 高 斯 牛顿 法 的 思想 并 且 通 过 自己 的 改进 修正 其 缺点 。 例 如 一 些 
线 搜索 方法 (line search method)， 这 类 改进 就 是 加 入 了 一 个 标量 a ， 在 确 
ET Ax 后 进一步 找到 a 使 得 /f(x +a Ax) ?达到 最 小 ， 而 不 是 像 高 斯 牛 
顿 法 那样 简单 地 令 w =1。 

列 文 伯 格 一 马 夸 尔 特 方法 在 一 定 程度 上 修正 了 这 些 问 题 ， 一 般 认 为 
它 比 高 斯 牛顿 法 更 为 健壮 。 尽 管 它 的 收敛 速度 可 能 会 比 高 斯 牛顿 法 更 
慢 ， 被 称 为 阻尼 牛顿 法 (Damped Newton Method) ， 但 是 在 SLAM 里 面 
却 被 大 量 应 用 。 


6.2.3” 列 文 伯 格 一 马 夸 尔 特 方 法 


由 于 高 斯 牛顿 法 中 采用 的 近似 二 阶 泰勒 展开 只 能 在 展开 点 附近 有 和 较 
好 的 近似 效果 ， 所 以 我 们 很 自然 地 想到 应 该 给 Ax 添加 一 个 信赖 区 域 
(Trust Region) ， 不 能 让 它 太 大 而 使 得 近似 不 准确 。 非 线性 优化 中 有 一 
系列 这 类 方法 ， 这 类 方法 也 被 称 为 信赖 区 域 方法 (Trust Region 
Method) 。 在 信赖 区 域 里 边 ， 我 们 认为 近似 是 有 效 的 ; 出 了 这 个 区 域 ， 
近似 可 能 会 出 问题 。 

那么 如 何 确定 这 个 信赖 区 域 的 范围 呢 ? 一 个 比较 好 的 方法 是 根据 我 
们 的 近似 模型 跟 实 际 阔 数 之 间 的 差异 来 确定 : MRAN, RALE 
围 尽 可 能 大 ， 如 果 差 异 大 ， 我 们 就 缩小 这 个 近似 范围 。 因 此 ， 考 虑 使 用 


_ f (æ+ Ag) - f (x) 
P= J (x) Ax 


来 判断 泰勒 近似 是 否 够 好 。p 的 分 子 是 实际 阔 数 下 降 的 值 ， 分 母 是 近 
似 模 型 下 降 的 值 。 如 果 p 接近 于 1， 则 近似 是 好 的 。 如 果 p 太 小 ， 说 明 实 
际 减 小 的 值 远 少 于 近似 减 小 的 值 ， 则 认为 近似 比较 差 ， 需要 缩小 近似 范 
围 。 反 之 ， 如 果 p 比较 大 ， 则 说 明 实 际 下 降 的 比 预计 的 更 大 ， 我 们 可 以 放 
大 近似 范围 。 

于 是 ， 我 们 构建 一 个 改良 版 的 非 线性 优化 框架 ， 该 框架 会 比 高 斯 牛 
顿 法 有 更 好 的 效果 : 

1. 给 定 初始 值 x* 。， 以 及 初始 优化 半径 。 


2. 对 于 第 k 次 和 迭代， 求解 : 


(6.23) 


os eyes ; 
min zll (£k) + J (£k) Argl,  s.t.||DAag||? < p, (6.24) 


这 里 4 是 信赖 区 域 的 半径 ，D 将 在 后 文 说 明 。 

3. 计 算 p 。 

4. 若 bp > 3. Mu =2p o 

5. 若 p< 13， 则 jp =0. 5p o 

6. 如 果 p 大 于 某 阅 值 ， 则 认为 近似 可 行 。 令 x, ,, =x, tAx, 。 


7. 判 断 算 法 是 否 收 侠 。 如 不 收 全 则 返回 第 2 步 ， 否 则 结束 。 


这 里 近似 范围 扩大 的 倍数 和 阅 值 都 是 经 验 值 ， 可 以 替换 成 别 的 数 
值 。 在 式 (6.24) 中 ， 我 们 把 增 量 限定 于 一 个 半径 为 的 球 中 ， 认 为 只 在 这 
TERA CARMAN. HED 之 后 ， 这 个 球 可 以 看 成 一 个 椭 球 。 在 列 文 但 
格 提出 的 优化 方法 中 ， 把 P 取 成 单位 阵 1 ， 相 当 于 直接 把 Ax 约束 在 一 个 球 
中 。 随 后 ， 马 硅 尔 特 提 出 将 D 取 成 非 负 数 对 角 阵 一 一 实际 中 通 单 用 三 7 的 
对 角 元 素平 方 根 ， 使 得 在 梯度 小 的 维度 上 约束 范围 更 大 一 些 。 

不 论 如 何 ， 在 列 文 但 格 一 马 伟 尔 特 优化 中 ， 我 们 都 需要 解 式 (6.24) 那 
样 一 个 子 问题 来 获得 梯度 。 这 个 子 问 题 是 之 不 等 式 约束 的 优化 问题 ， 我 
们 用 拉 格 明日 乘 子 将 它 转 化 为 一 个 无 约束 优化 问题 : 


al > 入 , 
min 5 ||f (ex) + J (ax) Azxll + 5 DAzI. (6.25) 
Tk 


这 里 4 为 拉 格 朗 日 乘 子 。 类 似 于 高 斯 牛顿 法 中 的 做 法 ， 把 它 展开 后 ， 
我 们 发 现 该 问题 的 核心 仍 是 计算 增 量 的 线性 方程 : 


(H + AD'D) Az =g. (6.26) 


可 以 看 到 ， 增 量 方 程 相 比 于 高 斯 牛顿 法 ， 多 了 一 项 MD7D 。 如 果 考 虑 
它 的 简化 形式 ， 即 D =T ， 那 么 相当 于 求解 : 


(五 十 XT)Az=9. 


我 们 看 到 ， 当 参数 4 比较 小 时 ， 互 占 主 要 地 位 ， 这 说 明 二 次 近似 模型 
在 该 沁 围 内 是 比较 好 的 ， 列 文 伯 格 一 马 硅 尔 特 方法 更 接近 于 高 斯 牛顿 
Ho AAA, SACRA, A 占据 主要 地 位 ， 列 文 伯 格 一 马 硅 尔 特 方 
法 更 接近 于 一 阶梯 度 下 降 法 〈 即 最 速 下 降 ) ， 这 说 明 附近 的 二 次 近似 不 
够 好 。 列 文 伯 格 一 马 硅 尔 特 方法 的 求解 方式 ， 可 在 一 定 程度 上 避免 线性 
方程 组 的 系 效 窃 阵 的 非 奇 异 和 病态 问题 ， 提 供 更 稳定 、 更 准确 的 增 量 Ax 


° 


在 实际 中 ， 还 存在 许多 其 他 的 方式 来 求解 国 数 的 增 量 ， 例 如 Dog-Leg 
等 方法 。 我 们 在 这 里 所 介绍 的 ， 只 是 最 常见 而 且 最 基本 的 方式 ， 也 是 视 
觉 SLAM 中 用 得 最 多 的 方式 。 总 而 言 之 ， 非 线性 优化 问题 的 框架 ， 分 为 
Line Search 和 Trust Region 两 类 。Line Search 先 固定 搜索 方向 ， 然 后 在 该 方 
向 寻找 步 长 ， 以 最 速 下 降 法 和 高 斯 牛顿 法 为 代表 。 而 Trust Region 则 先 固 
定 搜索 区 域 ， 再 考虑 找 该 区 域内 的 最 优点 。 此 类 方法 以 列 文 伯 格 一 马 雁 
尔 特 方法 为 代表 。 实 际 问 题 中 ， 我 们 通常 选择 高 斯 牛顿 法 或 列 文 伯 格 一 
马 硅 尔 特 方法 作为 梯度 下 降 策略 。 


6.2.4 ”小结 


由 于 不 希望 这 本 书 变 成 一 本 让 人 觉得 头疼 的 数学 教科 书 ， 所 以 这 里 
只 罗列 了 最 常见 的 两 种 非 线 性 优化 方案 一 一 高 斯 牛顿 法 和 列 文 伯 格 一 马 
硅 尔 特 方 法 。 我 们 避 开 了 许多 数学 性 质 上 的 讨论 。 如 果 读 者 对 优化 感 兴 
趣 ， 可 以 进一步 阅读 专门 介绍 数值 优化 的 书籍 (这 是 一 个 很 大 的 课 
题 ) ， 例 如 [23]。 以 高 斯 牛顿 法 和 列 文 伯 格 一 马 夸 尔 特 方法 为 代表 的 优化 


方法 ， 在 很 多 开源 的 优化 库 中 都 已 经 实现 并 提供 给 用 户 ， 我 们 会 在 下 文 
进行 实验 。 最 优化 是 处 理 许多 实际 问题 的 基本 数学 工具 ， 不 光 在 视觉 
SLAM 中 起 着 核心 作用 ， 在 类 似 于 深度 学 习 等 其 他 领域 ， 它 也 是 求解 问题 
的 核心 方法 之 一 。 我 们 希望 读者 能 够 根据 自身 能 力 ， 去 了 解 更 多 的 最 优 
化 算法 。 

也 许 你 发 现 了 ， 无 论 是 高 斯 牛顿 法 还 是 列 文 但 格 一 马 硅 尔 特 方法 ， 
在 做 最 优化 计算 时 ， 都 需要 提供 变量 的 初始 值 。 你 也 许 会 问 到 ， 这 个 初 
始 值 能 否 随意 设置 ?当然 不 是 。 实 际 上 非 线性 优化 的 所 有 迭代 求解 方 
案 ， 都 需要 用 户 来 提供 一 个 展 好 的 初始 值 。 由 于 目标 钞 数 太 复杂 ， 导 致 
在 求解 空间 上 的 变化 难以 琢磨 ， 对 问题 提供 不 同 的 初始 值 往往 会 导致 不 
同 的 计算 结果 。 这 种 情况 是 非 线 性 优化 的 通病 : 大 多 数 算法 都 容易 陷入 
局 部 极 小 值 。 因 此 ， 无 论 是 哪 类 科学 问题 ， 我 们 提供 初始 值 都 应 该 有 科 
学 依据 ， 例 如 视觉 SLAM 问 题 中 ， 我 们 会 用 ICP、PnP 之 类 的 算法 提供 优 
化 初始 值 。 总 之 ， 一 个 良好 的 初始 值 对 最 优化 问题 非常 重要 ! 


也 许 读 者 还 会 对 上 面 提 到 的 最 优化 产生 疑问 : 如 何 求解 线性 增 量 方 
程 组 呢 ? 我 们 只 讲 到 了 增 量 方程 是 一 个 线性 方程 ， 但 是 直接 对 系数 算 阵 
进行 求 逆 央 不 是 要 进行 大 量 的 计算 ? 当然 不 是 。 在 视觉 SLAM 算 法 里 ， 经 
单 遇 到 Ax 的 维度 大 到 好 几 百 或 者 上 千 ， 如 果 你 是 要 做 大 规模 的 视觉 三 维 
重建 ， 就 会 经 党 发 现 这 个 维度 可 以 轻易 达到 几 十 万 甚至 更 高 的 级 别 。 要 
对 那么 大 个 和 矩阵 进行 求 逆 是 大 多 数 处 理 器 无 法 负担 的 ， 因 此 存在 着 许多 
针对 线性 方程 组 的 数值 求解 方法 。 在 不 同 的 领域 有 不 同 的 求解 方式 ， 但 
J UF i APs Te BOK Fe SFE EAE, Ee RK FA ABBE AAAS A 
来 解 线 性 方程 ， 例 如 QR、Cholesky 等 分 解 方 法 。 这 些 方 法 通常 在 和 矩阵 论 
等 教科 书 中 可 以 找到 ， 我 们 不 多 加 介绍 。 


平 运 的 是 ， 视 觉 SLAM 里 这 个 和 矩阵 往往 有 特定 的 稀 焉 形式 ， 这 为 实时 
求解 优化 问题 提供 了 可 能 性 。 我 们 将 在 第 10 讲 中 详细 介绍 它 的 原理 。 利 
用 称 牙 形式 的 消 元 、 分 解 ， 最 后 再 进行 求解 增 量 ， 会 让 求解 的 效率 大 大 
提高 。 在 很 多 开源 的 优化 库 上 ， 维 度 为 一 万 多 的 变量 在 一 般 的 PC 上 就 可 
以 在 几 秒 甚至 更 短 的 时 间 内 被 求解 出 来 ， 其 原因 也 是 用 了 更 加 高 级 的 数 
学 工具 。 视 觉 SLAM 算 法 现在 能 够 实时 地 实现 ， 也 多 亏 了 系数 矩阵 是 稀 跑 
的 ， 如 果 和 矩 阵 是 币 密 的 ， 恐 怕 优 化 这 类 视觉 SLAM 算 法 就 不 会 被 学 界 广泛 


采纳 了 [24,25,26] s 


6.3 ”实践 : Ceres 


我 们 前 面 说 了 很 多 理论 ， 现 在 来 实践 一 下 前 面 提 到 的 优化 算法 。 在 
本 讲 的 实践 部 分 中 ， 我 们 主要 向 大 家 介绍 两 个 C++ 的 优化 库 : 来 自 合 歌 的 
Ceres 库 2 以 及 基于 图 优化 的 g2o 库 2 。 由 于 g20 的 使 用 还 需要 介绍 一 点 图 
优化 的 相关 知识 ， 所 以 我 们 先 来 介绍 Ceres， 然 后 介绍 一 些 图 优化 理论 ， 
最 后 来 讲 g2o。 由 于 优化 算法 在 之 后 的 “视觉 里 程 计 >” 和 * 后 端 ” 中 都 会 出 
现 ， 所 以 请 读者 务必 掌握 优化 算法 的 意义 ， 理 解 程序 的 内 容 。 


6.3.1 Ceres 简介 


Ceres 库 面向 通用 的 最 小 二 乘 问题 的 求解 ， 作 为 用 户 ， 我 们 需要 做 的 
就 是 定义 优化 问题 ， 然 后 设置 一 些 选 项 ， 输 入 进 Ceres 求 解 即 可 。 Ceres 求 
解 的 最 小 二 乘 问题 最 一 般 的 形式 如 下 〈 带 边界 的 核 函 数 最 小 二 乘 ) : 


. 2 
min 3) pi (Ili (Tirs Ein) ) 
2 


s.t. lj < Tj < Uj. 


可 以 看 到 ， 目 标 函 数 由 许多 平方 项 经 过 一 个 核 函 数 p (: ) 之 后 求 和 组 
成 mm 。 在 最 简单 的 情况 下 ， 取 p 为 恒 等 遂 数 ， 则 目标 水 数 即 为 许多 项 的 平 
方 和 。 在 这 个 问题 中 ， 优 化 变量 为 x ，,…,x,，f 称 为 代价 函数 (Cost 
function) ， 在 SLAM 中 亦 可 理解 为 误差 项 。1 Mu 为 第 ) 个 优化 变量 的 上 
限 和 下 限 。 在 最 简单 的 情况 下 ， 取 1 =-cou =% (不 限制 优化 变量 的 边 
R) ， 并 且 取 p 为 恒 等 函 数 时 ， 就 得 到 了 无 约束 的 最 小 二 乘 问题 ， 和 我 们 
先前 说 的 是 一 致 的 。 

在 Ceres 中 ， 我 们 将 定义 优化 变量 x 和 每 个 代价 遂 数 f ， 再 调用 Ceres 
进行 求解 。 我 们 可 以 选择 使 用 高 斯 牛顿 法 或 者 列 文 伯 格 一 马 硅 尔 特 方法 
进行 梯度 下 降 ， 并 设 定 梯度 下 降 的 条 件 ，Ceres 会 在 优化 之 后 将 最 优 估 计 
值 返回 。 下 面 ， 我 们 通过 一 个 曲线 拟 合 的 实验 来 实际 操作 一 下 Ceres， 理 
解 优 化 的 过 程 。 


(6.27) 


6.3.2 ”安装 Ceres 


为 了 使 用 Ceres， 首 先 需 要 进行 编译 安装 ! 建议 去 GitHub 上 下 载 
Ceres: https://github.com/ceres-solver/ceres- ice 本 书 资 产 的 3rdparty 下 
也 附带 了 Ceres 库 。 


与 之 前 位 到 的 库 一 样 ，Ceres 是 一 个 cmake 工 程 。 先 来 安装 它 的 依赖 
项 ， 在 Ubuntu 中 可 以 用 apt-get 安 装 ， 主 要 是 谷歌 自己 使 用 的 一 些 日 志和 测 
试 工 具 : 


1 | sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.2 libgflags- 
dev libgoogle-glog-dev libgtest-dev 


然后 ， 进 pantie 使 用 cmake 编 译 并 安装 它 。 这 个 过 程 我 
们 已 经 做 过 很 多 遍 了 ， 此 处 不 再 蒙 述 。 安装 完成 后 ， 
在 jusr/local/include/ceres 下 找到 Ceres 的 头 文件 ， 并 在 ned de sare 
为 libceres.a 的 库 文 件 。 有 了 这 些 文 件 ， 就 可 以 使 用 Ceres 进 行 优化 计算 
Te 


6.3.3 ”使 用 Ceres 拟 合 曲 线 


我 们 的 演示 实验 包括 使 用 Ceres 和 接 下 来 的 g2o 进 行 曲线 拟 合 。 假 设 有 
一 条 满足 以 下 方程 的 曲线 : 


y = explaz? + ba +c) +w 


其 中 a,b,c 为 曲线 的 参数 ，w 为 高 斯 噪声 。 我 们 改 意 选择 了 这 样 一 
非 线 性 模型 ， a 现在 ， 假 i ee 
观测 数据 点 ， 想 根据 这 些 e 数 据点 求 出 曲线 的 参数 。 那么 ， 可 以 求解 下 面 
的 最 小 二 乘 问题 以 估计 曲线 参数 : 


min 一 P lyi — exp (ax? + bz; + + c) I". (6.28) 


a,b,c 2 


请 注意 ， 在 这 个 问题 中 ， 待 估计 的 变量 是 bc ， 而 不 是 x 。 我 们 写 
一 个 程序 ， 先 根据 模型 生成 x,y 的 真 值 ， 然 后 在 真 值 中 添加 高 斯 分 布 的 噪 
声 。 随 后 ， 使 用 Ceres 从 融 品 声 的 数据 拟 合 参数 模型 。 


四 slambook/ch6/ceres_curve_ fi tting/main.cpp 


#include <iostream> 
#include <opencv2/core/core.hpp> 
#include <ceres/ceres.h> 


#include <chrono> 


using namespace std; 


// 代价 函数 的 计算 模型 
struct CURVE_FITTING_COST 


{ 


CURVE_FITTING_COST ( double x, double y ) : x (x), _y (Cy) {} 
// 残 差 的 计算 

template <typename T> 

bool operator() ( 


const T* const abc, // 模型 参数 ， 有 3 维 
T* residual ) const // RE 
{ 
// y-exp (az 22+bZ+c) 
residual[0] = T ( _y ) - ceres::exp (abc[0]*T ( x ) *T ( _x ) + abc[1]*T 
( _x ) + abc[2] ); 
return true; 
} 


const double _x, _y; // 2,y 数据 


int main ( int argc, char** argv ) 


{ 


double a=1.0, b=2.0, c=1.0; // 真实 参数 值 

int N=100; // 数据 点 

double w_sigma=1.0; // %&P Sigma 值 
cv::RNG rng; // OpenCV 随机 数 产生 器 
double abc[3] = {0,0,0}; // abe 套数 的 估计 值 
vector<double> x_data, y_data; // 数据 


cout<<"generating data: "<<endl; 


for ( int i=0; i<N; i++ ) 


{ 
double x = i/100.0; 
x_data.push_back ( x ); 
y_data.push_back ( 
exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma ) 
); 
cout<<x_data[i]<<" "<<y_data[i]<<end1l; 
{ 


// 构建 最 小 二 乘 问题 
ceres::Problem problem; 


for ( int i=0; i<N; i++ ) 


{ 
problem. AddResidualBlock ( // 向 问题 中 添加 误差 项 
// 使 用 自动 求 导 ， 模 板 参 数 : 误差 类 型 ， 输出 维度 ， 输 入 维度 ， 数 值 参 照 前 面 
struct 中 写法 
new ceres::AutoDiffCostFunction<CURVE_FITTING_C0ST，1，3> (人 
new CURVE_FITTING_COST ( x_data[i], y_data[i] ) 
Js 
nullptr, // KBR, EREA, A 
abc // 待 估计 参数 
) ; 
} 


// 配置 求解 器 
ceres::Solver::0ptions options; // 这 里 有 很 多 配置 项 可 以 填 
options .1linear_solver_type = ceres::DENSE_QR; // 增 量 方程 如 何 求解 


options.minimizer_progress_to_stdout = true; // $i H 3) cout 


ceres::Solver::Summary summary; AMV 优化 信息 
chrono: :steady_clock::time_point t1 = chrono::steady_clock: :now(); 
ceres::Solve ( options, &problem, &summary ); // 开始 优化 


chrono: :steady_clock::time_point t2 = chrono::steady_clock: :now(); 


69 chrono: :duration<double> time_used = chrono::duration_cast<chrono: :duration< 
double>>( t2-t1 ); 

70 cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl; 

71 

72 // 输出 结果 

73 cout<<summary.BriefReport() <<endl; 

74 cout<<"estimated a,b,c = "; 

75 for ( auto a:abc ) cout<<a<<" "; 

76 cout<<end1; 

TP 

78 return 0; 

79 } 


程序 中 需要 说 明 的 地 方 均 已 加 注释 。 可 以 看 到 ， 我 们 利用 OpenCV 的 
噪声 生成 器 生成 了 100 个 带 高 斯 噪声 的 数据 ， 随 后 利用 Ceres 进 行 拟 合 。 
Ceres 的 用 法 如 下 : 


1. 定 义 Cost Function 模 型 。 方 法 是 书写 一 个 类 ， 并 在 类 中 定义 带 模 板 
参数 的 0 运算 符 ， 这 样 该 类 就 成 为 了 一 个 拟 函 数 (Functor，C++ 术 语 ) © 
这 种 定义 方式 使 得 Ceres 可 以 像 调 用 图 数 一 样 ， 对 该 类 的 某 个 对 象 (比如 
a) 调用 a<double>() 方 法 这 使 对 象 具 有 像 函 数 那样 的 行为 。 


2. 调 用 AddResidualBlock 将 误差 项 添加 到 目标 水 数 中 。 由 于 优化 需要 
梯度 ， 我 们 有 若干 种 选择 : (1) 使 用 Ceres 的 自动 求 导 (Auto Di ff) ; 
(2) 使 用 数值 求 导 (Numeric Diff) ; (3) 自行 推导 解析 的 导数 形式 ， 
提供 给 Ceres。 其 中 自动 求 导 在 编码 上 是 最 方便 的 ， 于 是 我 们 使 用 自动 求 


= 
sto 


3. 自 动 求 导 需要 指定 误差 项 和 优化 变量 的 维度 。 这 里 的 误差 是 标量 ， 
维度 为 1， 优 化 的 是 a,b,c 三 个 量 ， 维 度 为 3。 于 是 ， 在 上 自动 求 导 类 的 模板 
参数 中 设 定 变量 维度 为 1、3。 

4. 设 定好 问题 后 ， 调 用 solve 函 数 进 行 求解 。 你 可 以 在 options 里 配置 

(非常 详细 的 ) 优化 选项 。 例 如 ， 可 以 选择 使 用 Line Search 还 是 Trust 
Region、 和 迭代 次 数 、 步 长 ， 等 等 。 读 者 可 以 查看 Options 的 定义 ， 看 看 有 
哪些 优化 方法 可 选 ， 当 然 默 认 的 配置 已 经 可 用 于 很 广泛 的 问题 了 。 


最 后 ， 我 们 来 看 看 实验 结果 。 调 用 build/curve_fi tting 查 看 优化 结 
R: 


1 |% build/curve_fitting 
2 | generating data: 

3 |O 2.71828 

4 |0.01 2.93161 

5 |0.02 2.12942 

6 |0.03 2.46037 


8 | iter cost cost_change |gradient | 


iter_time total_time 


9 |O 1.824887e+04 0.00e+00 1.38e+03 
4.09e-05 1.48e-04 
10 |1 2.748700e+39 -2.75e+39 0.00e+00 
1.09e-04 3.13e-04 
u |2 2.429783e+39 -2.43e+39 0.00e+00 
3.57e-05 3.75e-04 


13 |18 5.310764e+01 3.42e+00 8.50e+00 
3.09e-05 1.15e-03 

4 |19 5.125939e+01 1.85e+00 2.84e+00 
2.85e-05 1.19e-03 

15 |20 5.097693e+01 2.82e-01 4.34e-01 
2.82e-05 1.23e-03 

16 |21 5.096854e+01 8.39e-03 3.24e-02 
3.04e-05 1.27e-03 

17 | solve time cost = 0.00133349 seconds. 


|step| tr_ratio tr_radius ls_iter 
0.00e+00 0.00e+00 1.00e+04 0 
7.67e+01 -1.52e+35 5.00e+03 1 
7.62e+01 -1.35e+35 1.25e+03 1 
2.81e-01 9.89e-01 2.53e+03 1 
2.98e-01 9.90e-01 7.60e+03 1 
1.48e-01 9.95e-01 2.28e+04 1 
2.87e-02 9.96e-01 6.84e+04 1 


is | Ceres Solver Report: Iterations: 22, Initial cost: 1.824887e+04, Final cost: 


5.096854e+01, Termination: CONVERGENCE 


19 |estimated a,b,c = 0.891943 2.17039 0.944142 


从 Ceres 给 出 的 优化 过 程 可 以 看 到 ， 整 体 误差 大 约 从 18248 下 降 到 了 
50.9， 并 且 梯 度 也 是 越 来 越 小 。 在 达 代 22 次 后 算法 收 侠 ， 最 后 的 估计 值 为 


a = 0.891943, b = 2.17039, c=0.944142. 


而 我 们 设 定 的 真 值 为 


它们 相差 不 多 。 


为 了 更 直观 地 显示 数据 ， 可 以 把 它 画 出 来 ， 如 图 6-1 所 示 。 其 中 显示 
了 市 噪声 的 数据 、 真 实 模型 和 估计 模型 ， 可 以 看 到 估计 模型 和 真实 模型 
非常 接近 ， 几 乎 重合 。 我 们 同时 记录 了 Ceres 的 运行 时 间 ， 对 这 样 一 个 100 
个 点 的 优化 问题 ， 计 算 时 间 约 1.3ms (虚拟 机 上 ) o 


望 读者 通过 这 个 简单 的 例子 对 Ceres 的 使 用 方法 有 一 个 大 致 了 解 。 
它 的 优点 是 提供 了 自动 求 导 工具 ， 使 得 不 必 去 计算 很 及 烦 的 雅 可 比 和 矩 
阵 。 Ceres 的 自动 求 导 是 通过 模板 元 实现 的 ， 在 编译 时 期 就 可 以 完成 自动 
求 导 工作 ， 不 过 仍然 是 数值 导数 。 本 书 大 部 分 时 候 仍然 会 介绍 雅 可 比 矩 
阵 的 计算 ， 因 为 那样 对 理解 问题 更 有 帮助 ， 而 且 在 优化 中 更 少 出 现 问 
题 。 此 外 ，Ceres 的 优化 过 程 配置 也 很 丰富 ， 使 其 适合 很 广泛 的 最 小 二 乘 
优化 问题 ， 包 括 SLAM 中 的 各 种 问题 。 


图 6-1 ”使 用 Ceres 进 行 曲线 拟 合 。 真 实 模型 和 估计 模型 非常 接近 。 


6.4 实践: 920 


本 讲 的 第 2 个 实践 部 分 将 介绍 另 一 个 (主要 在 SLAM 领 域 ， 广 为 使 用 
的 优化 库 : g20 (General Graphic Optimization, G° O) 。 它 是 一 个 基于 图 
优化 的 库 。 图 优化 是 一 种 将 非 线 性 优化 与 图 论 结合 起 来 的 理论 ， 因 此 在 
使 用 它 之 前 ， 我 们 花 一 点 篇 幅 介 绍 一 下 图 优化 理论 。 


6.4.1 ”图 优化 理论 简介 


我 们 已 经 介绍 了 非 线 性 最 小 二 乘 的 求解 方式 。 它 们 是 由 很 多 个 误差 
项 之 和 组 成 的 。 然 而 ， 仅 有 一 组 优化 变量 和 许多 个 误差 项 ， 我 们 并 不 清 
楚 它 们 之 间 的 关联 。 比 如 ， 某 个 优化 变量 x 存在 于 多 少 个 误差 项 中 呢 ? 
我 们 能 保证 对 它 的 优化 是 有 意义 的 吗 ? 进一步 ， 我 们 希望 能 够 直观 地 看 
到 该 优化 问题 长 什么 样 。 于 是 ， 就 牵涉 到 了 图 优化 。 


图 优化 ， 是 把 优化 问题 表现 成 图 (Graph) 的 一 种 方式 。 这 里 的 图 
是 图 论 意义 上 的 图 。 一 个 图 由 若干 个 顶点 (Vertex) ， 以 及 连接 着 这 些 
TRA (Edge) 组 成 。 进 而 ， 用 顶点 表示 优化 变量 ， 用 边 表示 误差 
项 。 于 是 ， 对 任意 一 个 上 述 形式 的 非 线性 最 小 二 乘 问 题 ， 我 们 可 以 构建 
与 之 对 应 的 一 个 图 。 


图 6-2 是 一 个 简单 的 图 优化 例子 。 我 们 用 三 角形 表示 相机 位 资 世 点 ， 
用 圆 形 表示 路 标点 ， 它 们 构成 了 图 优化 的 顶点 ; 同时 ， 实 线 表示 相机 的 
运动 模型 ， 虚 线 表 示 观 测 模型 ， 它 们 构成 了 图 优化 的 边 。 此 时 ， 虽然 整 
个 问题 的 数学 形式 仍 是 式 (6.12) 那 样 ， 但 现在 我 们 可 以 直观 地 看 到 问题 的 
结构 了。 如果 希 望 ， 也 可 以 做 去 掉 孤 立 顶 点 或 优先 优化 边 数 较 多 (或 按 
图 论 的 术语 ， 度 数 较 大 ) 的 顶点 这 样 的 改进 。 但 是 最 基本 的 图 优化 是 用 
图 模型 来 表达 一 个 非 线性 最 小 二 乘 的 优化 问题 。 而 我 们 可 以 利用 图 模型 
的 某 些 性 质 做 更 好 的 优化 。 
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图 6-2 图 优化 的 例子 。 


8 多 0 为 SLAM 提 供 了 图 优化 所 需 的 内 容 。 下 面 演示 一 下 g2o 的 使 用 方 
法 。 


6.4.2 ”g20 的 编译 与 安装 


在 使 用 一 个 库 之 前 ， 我 们 需要 对 它 进 行 编 译 和 安装 。 读 者 应 该 已 经 
体验 过 很 多 次 这 种 过 程 了 ， 它 们 基本 大 同 小 异 。 关 于 g20， 读 者 可 以 从 
GitHub 下 载 它 : https://github.com/RainerKuemmerle/g20， 或 从 本 书 提供 的 
第 三 方 代码 库 中 获得 。 

解压 代码 包 后 ， 你 会 看 到 g20 库 的 所 有 源码 ， 它 也 是 一 个 cmake 工 
程 。 我 们 先 来 安装 它 的 依赖 项 (部 分 依赖 项 与 Ceres 重 合 ) : 


1 | sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev libsuitesparse-dev 
libcxsparse3.1.2 libcholmod-dev 


然后 ， 按 照 cmake 的 方式 对 g2o 进 行 编 译 安 装 即 可 ， 这 里 略 去 对 该 过 
程 的 说 明 。 安 装 完成 后 ，g20 的 头 文 件 将 位 于 /usr/local/g20 下 ， 库 文件 位 


于 /usr/local/lib/ 下 。 现 在 ， 我 们 重新 考虑 Ceres 例 程 中 的 曲线 拟 合 实验 ， 在 
g2o 中 实验 一 遍 。 


6.4.3 ”使 用 g2o 拟 合 曲线 


为 了 使 用 g20， 首 先 要 将 曲线 拟 合 问题 抽象 成 图 优化 。 这 个 过 程 中 ， 
只 要 记 住 节点 为 优化 变量 ， 边 为 误差 项 即 可 。 曲 线 拟 合 的 图 优化 问题 可 
以 画 成 图 6-3 的 形式 。 


C) 待 估计 的 参数 构成 了 节点 
观测 数据 构成 了 边 


图 6-3 ”曲线 拟 合 对 应 的 图 优化 模型 。 ( 莫 明 其 妙 地 有 些 像 华为 的 标志 ) 


在 曲线 拟 合 问题 中 ， 整 个 问题 只 有 一 个 顶点 : 曲线 模型 的 参数 a,b,c 
; 而 各 个 带 噪声 的 数据 点 ， 构 成 了 一 个 个 误差 项 ， 也 就 是 图 优化 的 边 。 
但 这 里 的 边 与 我 们 平时 想 的 边 不 太一 样 ， 它 们 是 一 元 边 (Unary 
Edge) ， 即 只 连接 一 个 顶点 一 一 因为 整个 图 只 有 一 个 顶点 。 所 以 在 图 6-3 
中 ， 我 们 只 能 把 它 画 成 自己 连 到 自己 的 样子 。 事 实 上 ， 图 优化 中 一 条 边 
可 以 连接 一 个 、 两 个 或 多 个 顶点 ， 这 主要 反映 每 个 误差 与 多 少 个 优化 变 
量 有 关 。 在 稍 有 些 玄妙 的 说 法 中 ， 我 们 把 它 叫 作 超 边 (Hyper Edge) ， 
整个 图 叫 作 超 图 (Hyper Graph) Po 

弄 清 了 这 个 图 模型 之 后 ， 接 下 来 就 是 在 g2o 中 建立 该 模型 进行 优化 
了 。 作 为 g2o 的 用 户 ， 我 们 要 做 的 事主 要 包含 以 下 步骤 : 


2. 构 建 图 。 

3. 选 择优 化 算法 。 

4. 调 用 g2o 进 行 优化 ， 返 回 结果 。 

下 面 演示 一 下 程序 。 

kÀ slambook/ch6/ g20_curve_ fi tting/main.cpp 


#include <iostream> 


#include <g2o0/core/base_vertex.h> 

#include <g2o/core/base_unary_edge.h> 

#include <g20/core/block_solver.h> 

#include <g2o0/core/optimization_algorithm_levenberg.h> 
#include <g2o/core/optimization_algorithm_gauss_newton.h> 
#include <g2o/core/optimization_algorithm_dogleg.h> 
#include <g20/solvers/dense/linear_solver_dense.h> 
#include <Eigen/Core> 

#include <opencv2/core/core.hpp> 

#include <cmath> 

#include <chrono> 


using namespace std; 


// 曲线 模型 的 顶点 ， 模 板 套数 : 优化 变量 维度 和 数据 类 型 
class CurveFittingVertex: public g20::BaseVertex<3, Eigen::Vector3d> 
{ 
public: 
EIGEN_MAKE_ALIGNED_OPERATOR_NEW 
virtual void setToOriginImpl() // 重 置 
{ 


_estimate << 0,0,0; 


virtual void oplusImpl( const double* update ) // 更 新 
{ 
_estimate += Eigen: :Vector3d(update) ; 
} 
// 存盘 和 读 盘 : BE 
virtual bool read( istream& in ) {} 
virtual bool write( ostream& out ) const {} 
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// 误差 模型 模板 参数 : 观测 值 维度 ， 类 型 ， 连 接 顶 点 类 型 
class CurveFittingEdge: public g20::BaseUnaryEdge<1,double,CurveFittingVertex> 
{ 
public: 
EIGEN_MAKE_ALIGNED_OPERATOR_NEW 
CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {} 
// 计算 曲线 模型 误差 
void computeError() 


{ 


const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> ( 


_vertices[0]); 
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const Eigen::Vector3d abc = v->estimate() ; 
_error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc 
(2,0) 2 ; 

} 

virtual bool read( istream& in ) {} 

virtual bool write( ostream& out ) const {} 

public: 
double _x; // x fii, y 值 为 _measurement 


a 


int main( int argc, char** argv ) 


{ 
double a=1.0, b=2.0, c=1.0; // 真实 参数 值 
int N=100; // 数据 点 
double w_sigma=1.0; // 噪声 Sigma 值 
cv::RNG rng; // 0penCV 随 机 数 产 生 器 
double abc[3] = {0,0,0}; // abc 参 数 的 估计 值 
vector<double> x_data, y_data; // 数据 


cout<<"generating data: "<<endl; 


for ( int i=0; i<N; i++ ) 


{ 
double x = i/100.0; 
x_data.push_back ( x ); 
y_data.push_back ( 
exp ( a¥*x*x + b*x + c ) + rng.gaussian ( w_sigma ) 
); 
cout<<x_data[i]<<" "<<y_data[i]<<endl; 
} 


// 构建 图 优化 ， 先 设 定 g20 

// BER: 每 个 误差 项 优化 变量 维度 为 3 ,误差 值 维度 为 1 

typedef g20::BlockSolver< g20::BlockSolverTraits<3,1> > Block; 

// 线性 方程 求解 器 : 稠密 的 增 量 方程 

Block: :LinearSolverType* linearSolver = new g20::LinearSolverDense<Block:: 
PoseMatrixType>() ; 

Block* solver_ptr = new Block( linearSolver ); // FETEI RIES 
// 梯度 下 降 方法 ， 从 GN，LM，DogLeg 中 选 
g2o::O0ptimizationAlgorithmLevenberg* solver = new g2o:: 
OptimizationAlgorithmLevenberg( solver_ptr ); 

// 取消 下 面 的 注释 以 使 用 GN 或 DogLeg 

// g20::OptimizationAlgorithmGaussNewton* solver = new g2o:: 
OptimizationAlgorithmGaussNewton( solver_ptr ); 


// g20::OptimizationAlgorithmDogleg* solver = new g2o:: 
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OptimizationAlgorithmDogleg( solver_ptr ); 


g20::SparseOptimizer optimizer; // 图 模型 
optimizer.setAlgorithm( solver ); // 设置 求解 器 
optimizer.setVerbose( true ); // 打开 调试 输出 


// 向 图 中 增加 顶点 

CurveFittingVertex* v = new CurveFittingVertex() ; 
v->setEstimate( Eigen: :Vector3d(0,0,0) ); 
v->setId(0); 


optimizer.addVertex( v ); 


// 向 图 中 增加 边 

for ( int i=0; i<N; i++ ) 

{ 
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] ); 
edge->setId(i); 
edge->setVertex( 0, v ); // 设置 连接 的 顶点 
edge->setMeasurement( y_data[i] ); // 观测 数值 
// 信息 矩阵 : 协 方差 矩阵 之 逆 
edge->setInformation( Eigen: :Matrix<double,1,1>::Identity()*1/(w_sigma* 
w_sigma) ); 
optimizer.addEdge( edge ); 


AMV 执行 优化 

cout<<"start optimization"<<endl; 

chrono::steady_clock::time_point t1 chrono::steady_clock: :now(); 
optimizer.initialize0ptimization() ; 


optimizer.optimize(100) ; 


chrono: :steady_clock::time_point t2 = chrono::steady_clock::now(); 


chrono: :duration<double> time_used = chrono: :duration_cast<chrono: :duration< 
double>>( t2-t1 ); 


cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl; 
// 输出 优化 值 
Eigen::Vector3d abc_estimate = v->estimate(); 


cout<<"estimated model: "<<abc_estimate. transpose ()<<end1; 


return 0; 


在 这 个 程序 中 ， 我 们 从 g2o 派 生出 了 用 于 曲线 拟 合 的 图 优化 顶点 和 
边 : CurveFittingVertex 和 CurveFittingEdge， 这 实质 上 扩展 了 g2o 的 使 用 方 
式 。 在 这 两 个 派生 类 中 ， 我 们 重 写 了 重要 的 虚 阔 数 : 


1AA EMAZ: oplusImpl。 我 们 知道 优化 过 程 最 重要 的 是 增 量 Ax 
的 计算 ， 而 该 函数 处 理 的 是 x ,, =x, +Ax 的 过 程 。 读 者 也 许 觉 得 这 并 不 是 
什么 值得 一 提 的 事情 ， 因 为 仅仅 是 个 简单 的 加 法 而 已 ， 为 什么 g2o 不 帮 有 我 
们 完成 呢 ? 在 曲线 拟 合 过 程 中 ， 由 于 优化 变量 (曲线 参数 ) 本 身 位 于 向 
量 空间 中 ， 这 个 更 新 计算 确实 就 是 简单 的 加 法 。 但 是 ， 当 优化 变量 不 在 
向 量 空间 中 时 ， 比 如 说 x 是 相机 位 姿 ， 它 本 身 不 一 定 有 加 法 运算 。 这 时 ， 
就 需要 重新 定义 增 量 如 何 加 到 现 有 的 估计 上 的 行为 了 。 按 照 第 4 讲 的 解 
释 ， 我 们 可 能 使 用 左 乘 更 新 或 右 乘 更 新 ， 而 不 是 直接 的 加 法 。 


2. 顶 点 的 重 置 函 数 : setToOriginImpl。 这 是 平凡 的 ， 我 们 把 估计 值 置 
零 即 可 。 


3. 边 的 误差 计算 函数 : computeError。 该 国 数 需要 取出 边 所 连接 的 顶 
点 的 当前 估计 值 ， 根 据 曲线 模型 ， 与 它 的 观测 值 进 行 比 较 。 这 和 最 小 二 
乘 问题 中 的 误差 模型 是 一 致 的 。 

4. 存 盘 和 读 盘 水 数 : read、write。 由 于 我 们 并 不 想 进 行 读 / 写 操作 ， 所 


以 留 空 。 


定义 了 顶点 和 边 之 后 ， 我 们 在 main 函 数 里 声明 了 一 个 图 模型 ， 然 后 
按照 生成 的 噪声 数据 ， 往 图 模型 中 添加 顶点 和 边 ， 最 后 调用 优化 冰 数 进 
行 优 化 。g2o 会 给 出 优化 的 结果 : 


1 |% build/curve_fitting 
2 | generating data: 

3 |0 2.71828 

4 |0.01 2.93161 

5 |0.02 2.12942 


7 | iteration= 13 chi2= 101.937020 time= 4.06e-05 cumTime= 0.00048135 edges= 100 
schur= 0 lambda= 3678.088107 levenbergIter= 6 

s | iteration= 14 chi2= 101.937020 time= 3.2215e-05 cumTime= 0.000513565 edges= 100 
schur= 0 lambda= 19616.469906 levenbergIter= 3 

9 | iteration= 15 chi2= 101.937020 time= 0.000108524 cumTime= 0.000622089 edges= 100 
schur= 0 lambda= 836969.382664 levenbergIter= 4 

10 | iteration= 16 chi2= 101.937020 time= 0.000159817 cumTime= 0.000781906 edges= 100 
schur= 0 lambda= 224672257893341.656250 levenbergIter= 7 

i | solve time cost = 0.00173976 seconds. 

12 |estimated model: 0.890911 2.1719 0.943629 


FX {6 FA (SSR ATE RB, EART 10 
后 ， 最 后 优化 结果 与 Ceres 实 验 中 相差 无 几 。 我 们 也 在 程序 中 提供 了 使 用 
高 斯 牛顿 法 和 DogLeg 下 降 方 式 ， 请 读者 去 掉 它 们 前 面 的 注释 符号 ， 目 行 
对 比 各 种 梯度 下 降 方 法 的 差异 。 


6.5 小结 


本 节 介 绍 了 SLAM 中 经 党 碰 到 的 一 种 非 线性 优化 问题 : 由 许多 个 误差 
项 平方 和 组 成 的 最 小 二 乘 问题 。 我 们 介绍 了 它 的 定义 和 求解 ， 并 且 讨 论 
了 两 种 主要 的 梯度 下 降 方 式 : 高 斯 牛顿 法 和 列 文 伯 格 一 马 硅 尔 特 方法 。 
在 实践 部 分 中 ， 分 别 使 用 了 Ceres 和 g2o 两 种 优化 库 求解 同一 个 曲线 拟 合 问 
题 ， 发 现 它们 给 出 了 相似 的 结果 。 


由 于 还 没有 详细 谈 Bundle Adjustment， 所 以 实践 部 分 选择 了 曲线 拟 合 
这 样 一 个 简单 但 有 代表 性 的 例子 ， 以 演示 一 般 的 非 线性 最 小 二 乘 求解 方 
式 。 特 别 地 ， 如 果 用 g2o 来 拟 合 曲线 ， 必 须 先 把 问题 转换 为 图 优化 ， 定 义 
新 的 顶点 和 边 ， 这 种 做 法 是 有 一 些 迁 回 的 一 一 g2o 的 主要 目的 并 不 在 此 。 
相 比 之 下 ，Ceres 定 义 误 差 项 求 曲线 拟 合 问题 则 目 然 了 很 多 ， 因 为 它 本 身 
即 是 一 个 优化 库 。 然 而 ， 在 SLAM 中 更 多 的 问题 是 ， 一 个 带 有 许多 个 相机 


位 姿 和 许多 个 空间 点 的 优化 问题 如 何 求解 。 特 别 地 ， 当 相机 位 姿 以 李 代 
数 表示 时 ， 误 差 项 关于 相机 位 姿 的 导数 如 何 计 算 ， 将 是 一 件 值得 详细 讨 
论 的 事 。 我 们 将 在 后 续 内 容 发 现 ，g2o 提 供 了 大 量 的 顶点 和 边 的 类 型 ， 非 
常 便 于 相机 位 姿 估 计 问 题 。 而 在 Ceres 中 ， 我 们 不 得 不 自己 实现 每 一 个 
Cost Function， 有 一 些 不 便 。 

在 实践 部 分 的 两 个 程序 中 ， 我 们 没有 去 计算 曲线 模型 关于 三 个 参数 
的 导数 ， 而 是 利用 了 优化 库 的 数值 求 导 ， 这 使 得 理论 和 代码 都 会 简洁 一 
He, Ceres 库 提供 了 基于 模板 元 的 自动 求 导 和 运行 时 的 数值 求 导 ， 而 g20 只 
提供 了 运行 时 数值 求 导 这 一 种 方式 。 但 是 ， 对 于 大 多 数 问 题 ， 如 果 能 够 
推导 出 雅 可 比 和 矩阵 的 解析 形式 并 告诉 优化 库 ， 就 可 以 避免 数值 求 导 中 的 
诸多 问题 。 

最 后 ， 希 望 读者 能 够 适应 Ceres 和 g20 这 些 大 量 使 用 模板 编程 的 方式 。 
也 许 一 开始 会 看 上 去 比较 吓人 (特别 是 Ceres 设 置 Problem 和 g20 初 始 化 部 
分 的 代码 ) ， 但 是 熟悉 之 后 ， 就 会 觉得 这 样 的 方式 是 自然 的 ， 而 且 容 易 
扩展 。 我 们 将 在 SLAM 后 端 一 讲 中 继续 讨论 稀疏 性 、 核 函数 、 位 姿 图 
(Pose Graph) 等 问题 。 

习题 

1. 证 明 线 性 方程 Ax =b 4 RABIA 超 定 时 ， 最 小 二 乘 解 为 x =(A7A) 
-1ATbo 

2. 调 研 最 速 下 降 法 、 牛 顿 法 、 高 斯 牛顿 法 和 列 文 伯 格 一 马 夸 尔 特 方法 
各 有 什么 优 缺 点 。 除 了 我 们 举 的 Ceres 库 和 g2o 库 ， 还 有 哪些 常用 的 优化 
E? (你 可 能 会 找到 一 些 MATLAB 上 的 库 。) 

3. 为 什么 高 斯 牛顿 法 的 增 量 方程 系数 和 矩阵 可 能 不 正定 ? 不 正定 有 什么 
几何 含义 ? 为 什么 在 这 种 情况 下 解 就 不 稳定 了 ? 

4.DogLeg 是 什么 ? 它 与 高 斯 牛顿 法 和 列 文 伯 格 一 马 硅 尔 特 方法 有 何 
异同 ? 请 搜索 相关 的 材料 中 。 

5. 阅 读 Ceres 的 教学 材料 (http://ceres-solver.org/tutorial.html) 以 更 好 
地 掌握 其 用 法 。 

6. 阅 读 g20 自 带 的 文档 ， 你 能 看 懂 它 吗 ? 如 果 还 不 能 完全 看 懂 ， 请 在 
第 10 讲 和 第 11 讲 之 后 回来 再 看 。 


7.* 请 更 改 曲线 拟 合 实验 中 的 曲线 模型 ， 并 用 Ceres 和 8g2o 进 行 优化 实 
验 。 例 如 ， 可 以 使 用 更 多 的 参数 和 更 复杂 的 模型 。 

[1] 核 函 数 的 详细 讨论 见 第 10 讲 。 

[2] 虽然 笔者 个 人 并 不 太 喜 欢 有 些 故弄玄虚 的 说 法 。 


[3] 例如 ，http:/www.numerical.rl.ac.uk/people/nimg/course/lectures/raphael/lectures/lec7slides.pdf。 
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主要 目标 


1. 理 解 图 像 特征 点 的 意义 ,并 掌握 在 单 幅 图 像 中 提取 出 特征 点 及 多 幅 
图 像 中 匹配 特征 点 的 方法 。 


2. 理 解 对 极 几 何 的 原理 ， 利 用 对 极 几何 的 约束 ， 恢 复出 图 像 之 间 的 摄 
像 机 的 三 维 运动 。 

3. 理 解 PNP 问 题 ， 以 及 利用 已 知 三 维 结构 与 图 像 的 对 应 关系 求解 摄像 
机 的 三 维 运动 。 

4. 理 解 ICP 问 题 ， 以 及 利用 点 云 的 匹配 关系 求解 摄像 机 的 三 维 运动 。 

5. 理 解 如 何 通过 三 角 化 获得 二 维 图 像 上 对 应 点 的 三 维 结构 。 


本 书 前 面 介 绍 了 运动 方程 和 观测 方程 的 具体 形式 ， 并 讲解 了 以 非 线 
性 优化 为 主 的 求解 方法 。 从 本 讲 开 始 ， 我 们 结束 基础 知识 的 铺垫 而 步 入 
正题 : 按照 第 2 讲 的 内 容 ， 分 别 介绍 视觉 里 程 计 、 优 化 后 端 、 回 环 检测 和 
地 图 构建 4 个 模块 。 本 讲 和 下 一 讲 主 要 介绍 作为 视觉 里 程 计 的 主要 理论 ， 
然后 在 第 9 讲 中 进行 一 次 实践 。 本 讲 关 注 基于 特征 点 方式 的 视觉 里 程 计算 
法 。 我 们 将 介绍 什么 是 特征 点 、 如 何 提取 和 匹配 特征 点 ， 以 及 如 何 根 据 
配对 的 特征 点 估计 相机 运动 。 


7.1 RIERA 


回顾 第 2 讲 的 内 容 ， 我 们 说 过 视觉 SLAM 主 要 分 为 视觉 前 端 和 优化 后 
端 。 前 端 也 称 为 视觉 里 程 计 (VO) 。 它 根据 相 令 图像 的 信息 估计 出 粗略 
的 相机 运动 ， 给 后 端 提 供 较 好 的 初始 值 。VO 的 实现 方法 ， 按 是 否 需要 提 
取 特 征 ， 分 为 特征 点 法 的 前 端 及 不 提 特 征 的 直接 法 前 端 。 基 于 特征 点 法 
的 前 端 ， 长 久 以 来 (直到 现在 ) 被 认为 是 视觉 里 程 计 的 主流 方法 。 它 运 
行 稳定 ， 对 光照 、 动 态 物 体 不 敏感 ， 是 目前 比较 成 熟 的 解决 方案 。 在 本 
讲 中 ， 我 们 将 从 特征 点 法 入 手 ， 学 习 如 何 提 取 、 匹 配 图 像 特征 点 ， 然 后 
估计 两 帧 之 间 的 相机 运动 和 场景 结构 ， 从 而 实现 一 个 基本 的 两 帧 间 视 觉 
里 程 计 。 


7.1.1 ”特征 点 


VO 的 主要 问题 是 如 何 根 据 图 像 来 估计 相机 运动 。 然 而 ， 图 像 本 身 是 
一 个 由 亮度 和 色彩 组 成 的 矩阵 ， 如 果 直 接 从 和 矩阵 层面 考虑 运动 估计 ， 将 
会 非常 困难 。 所 以 ， 我 们 习惯 于 采用 这 样 一 种 做 法 : 首先 ， 从 图 像 中 选 
取 比 较 有 代表 性 的 氮 。 这 些 点 在 相机 视角 发 生 少 量变 化 后 会 保持 不 变 ， 
所 以 我 们 会 在 各 个 图 像 中 找到 相同 的 点 。 然 后 ， 在 这 些 点 的 基础 上 ， 讨 
论 相 机 位 次 估计 问题 ， 以 及 这 些 点 的 定位 问题 。 在 经 典 SLAM 模 型 中 ， 把 
它们 称 为 路 标 。 而 在 视觉 SLAM 中 ， 路 标 则 是 指 图 像 特征 (Feature) o 

根据 维基 百科 的 定义 ， 图 像 特征 是 一 组 与 计算 任务 相关 的 信息 ， 计 
算 任务 取决 于 具体 的 应 用 中。 简 而 言 之 ， 特 征 是 图 像 信息 的 另 一 种 数字 
表达 形式 。 一 组 好 的 特征 对 于 在 指定 任务 上 的 最 终 表现 至 关 重 要 ， 所 以 
多 年 来 研究 者 们 花费 了 大 量 的 精力 对 特征 进行 研究 。 数 字 图 像 在 计算 机 
中 以 灰 度 值 矩 阵 的 方式 存储 ， 所 以 最 简单 的 ， 单 个 图 像 像 素 也 是 一 种 “ 特 
征 ”。 但是， 在 视觉 里 程 计 中 ， 我 们 希望 特征 点 在 相机 运动 之 后 保持 稳定 
， 而 灰 度 值 受 光照 、 形 变 、 物 体 材质 的 影响 严重 ， 在 不 同 图 像 间 变 化 非 
党 大 ， 不 够 稳定 。 理 想 的 情况 是 ， 当 场景 和 相机 视角 发 生 少量 改变 时 ， 
还 能 从 图 像 中 判断 哪些 地 方 是 同一 个 点 ， 因 此 仅 任 灰 度 值 是 不 够 的 ， 我 
们 需要 对 图 像 提 取 特 征 点 。 


特征 点 是 图 像 里 一 些 特别 的 地 方 。 以 图 7-1 为 例 。 我 们 可 以 把 图 像 中 
的 角 点 、 边 缘 和 区 块 都 当成 图 像 中 有 代表 性 的 地 方 。 不 过 ， 我 们 更 容易 


精确 地 指出 ， 某 两 幅 图 像 中 出 现 了 同一 个 角 上 点; 同一 个 边缘 则 稍微 困难 
一 些 ， 因 为 沿 着 该 边缘 前 进 ， 图 像 局 部 是 相似 的 ;同一 个 区 块 则 是 最 困 
难 的 。 我 们 发 现 ， 图 像 中 的 角 点 、 边 缘 相 比 于 像素 区 块 而 言 更 加 “ 特 
别 >， 在 不 同 图 像 之 间 的 辨识 度 更 强 。 所 以 ， 一 种 直观 的 提取 特征 的 方式 
就 是 在 不 同 图 像 间 辨 认 角 点 ， 确 定 它 们 的 对 应 关系 。 在 这 种 做 法 中 ， 角 
点 就 是 所 谓 的 特征 。 

然而 ， 在 大 多 数 应 用 中 ， 单 纯 的 角 点 依然 不 能 满足 我 们 的 很 多 需 
求 。 例 如 ， 从 远 处 看 上 去 是 角 点 的 地 方 ， 当 相机 走 近 之 后 ， 可 能 就 不 显 
示 为 角 点 了 。 或 者 ， 当 旋转 相机 时 ， 角 点 的 外 观 会 发 生变 化 ， 我 们 也 就 
不 容易 辨认 出 那 是 同一 个 角 点 。 为 此 ， 计 算 机 视觉 领域 的 研究 者 们 在 长 
年 的 研究 中 设计 了 许多 更 加 稳定 的 局 部 图 像 特征 ， 如 著名 的 SIFT® 、 
SURF®", ORB™ ， 等 等 。 相 比 于 朴素 的 角 点 ， 这 些 人 工 设计 的 特征 点 
能 够 拥有 如 下 的 性 质 : 

1. 可 重复 性 (Repeatability) : 相同 的 “区 域 ”* 可 以 在 不 同 的 图 像 中 找 
到 。 

2. 可 区 别 性 (Distinctiveness) : 不 同 的 “区 域 ? 有 不 同 的 表达 。 

3. 高 效率 (E ffi ciency) : 同一 图 像 中 ， 特 征 点 的 数量 应 远 小 于 像素 
的 数量 。 

4. 本 地 性 (Locality) : 特征 仅 与 一 小 片 图 像 区 域 相 关 。 


图 7-1 可 以 作为 图 像 特征 的 部 分 : 角 点 、 边 缘 、 区 块 


特征 点 由 关键 点 (Key-point) 和 描述 子 (Descriptor) 两 部 分 组 成 。 
比如 ， 当 谈论 SIFT 特 征 时 ， 是 指 “ 提 取 SIFT 关 键 点 ， 并 计算 SIFT 描 述 子 ” 
两 件 事情 。 关 键 点 是 指 该 特征 点 在 图 像 里 的 位 置 ， 有 些 特征 点 还 具有 朝 


向 、 大 小 等 信息 。 描 述 子 通常 是 一 个 向 量 ， 按 照 某 种 人 为 设计 的 方式 ， 
描述 了 该 关键 点 周围 像素 的 信息 。 描 述 子 是 按照 “外 观 相似 的 特征 应 该 有 
相似 的 描述 子 ”的 原则 设计 的 。 因 此 ， 只 要 两 个 特征 点 的 描述 子 在 向 量 空 
间 上 的 距离 相近 ， 就 可 以 认为 它们 是 同样 的 特征 点 。 


历史 上 ， 研 究 者 们 提出 过 许多 图 像 特征 。 它 们 有 些 很 精确 ， 在 相机 
的 运动 和 光照 变化 下 仍 具 有 相似 的 表达 ， 但 相应 地 需要 较 大 的 计算 量 。 
HH, SIFT (尺度 不 变 特征 变换 ，Scale-Invariant Feature Transform) 当 属 
最 为 经 典 的 一 种 。 它 充分 考虑 了 在 图 像 变 换 过 程 中 出 现 的 光照 、 尺 度 、 
旋转 等 变化 ， 但 随 之 而 来 的 是 极 大 的 计算 量 。 由 于 整个 SLAM 过 程 中 图 像 
特征 的 提取 与 匹配 仅仅 是 诸多 环节 中 的 一 个 ， 到 目前 (2016 年 ) ALE, 
普通 PC 的 CPU 还 无 法 实时 地 计算 SIFT 特 征 ， 进 行 定 位 与 建 图 。 所 以 在 
SLAM 中 我 们 甚 少 使 用 这 种 “奢侈 ”的 图 像 特征 。 


另 一 些 特 征 ， 则 考虑 适当 降低 精度 和 健壮 性 ， 以 提升 计算 的 速度 。 
例如 ，FAST 关 键 点 属于 计算 特别 快 的 一 种 特征 点 (注意 这 里 “关键 点 ”的 
表述 ， 说 明 它 没有 描述 子 ) 。 而 ORB (Oriented FAST and Rotated 
BRIEF) 特征 则 是 目前 看 来 非常 具有 代表 性 的 实时 图 像 特 征 。 它 改进 了 
FASTA MF 不 具有 方向 性 的 问题 ， 并 采用 速度 极 快 的 二 进 制 描述 子 
BRIEF“! ， 使 整个 图 像 特征 提取 的 环节 大 大 加 速 。 根 据 作 者 在 论文 中 所 
述 测试 ， 在 同一 幅 图 像 中 同时 提取 约 1000 个 特征 点 的 情况 下 ，ORB 约 要 
花费 15.3ms，SURF 约 花费 217.3ms，SIFT 约 花费 5228.7ms。 由 此 可 以 看 
出 ，ORB 在 保持 了 特征 子 具 有 旋转 、 尺 度 不 变性 的 同时 ， 速 度 方面 提升 
明显 ， 对 于 实时 性 要 求 很 高 的 SLAM 来 说 是 一 个 很 好 的 选择 。 


大 部 分 特征 提取 都 具有 较 好 的 并 行 性 ， 可 以 通过 GPU 等 设备 来 加 速 
计算 。 经 过 GPU 加 速 后 的 SIFT， 就 可 以 满足 实时 计算 要 求 。 但 是 ， 引 入 
GPU 将 融 来 整个 SLAM 成 本 的 提升 。 由 此 矶 来 的 性 能 提升 是 否 足以 抵 去 付 
出 的 计算 成 本 ， 需 要 系统 的 设计 人 员 仔 细 考 量 。 在 目前 的 SLAM 方 案 中 ， 
ORB 是 质量 与 性 能 之 间 较 好 的 折 中 ， 因 此 ， 我 们 以 ORB 为 代表 介绍 提取 
特征 的 整个 过 程 。 


7.1.2 ORB 特征 


ORB 特 征 亦 由 关键 点 和 描述 子 两 部 分 组 成 。 它 的 关键 点 称 为 
“Oriented FAST”， 是 一 种 改进 的 FAST 角 点 ， 关 于 什么 是 FAST 角 点 我 们 将 


在 下 文 介 绍 。 它 的 描述 子 称 为 BRIEF (Binary Robust Independent 
Elementary Feature) 。 因 此 ， 提 取 ORB 特 征 分 为 如 下 两 个 步骤 : 


L.FASTAA Rte: 找 出 图 像 中 的 “ 角 点 ”。 相 较 于 原版 的 FAST，ORB 
中 计算 了 特征 点 的 主 方向 ， 为 后 续 的 BRIEF 描 述 子 增加 了 旋转 不 变 特性 。 

2.BRIEF 描 述 子 : 对 前 一 步 提 取出 特征 点 的 周围 图 像 区 域 进行 描述 。 

下 面 分 别 介绍 FAST 和 BRIEF。 

FAST 关 键 点 


FAST 是 一 种 角 点 ， 主 要 检测 局 部 像素 灰 度 变 化 明显 的 地 方 ， 以 速度 
快 著称 。 它 的 思想 是 : 如 果 一 个 像素 与 邻 域 的 像素 差别 较 大 (过 亮 或 过 
暗 ) ， 那 么 它 更 可 能 是 角 点 。 相 比 于 其 他 角 点 检测 算法 ，FAST 只 需 比较 
像素 亮度 的 大 小 ， 十 分 快捷 。 它 的 检测 过 程 如 下 ( 见 图 7-2) : 


1. 在 图 像 中 选取 像素 p ， 假 设 它 的 亮度 为 1 o 
2. 设 置 一 个 阅 值 7 《比如 , 五 的 209%) o 


3. 以 像素 p 为 中 心 ， 选 取 半 径 为 3 的 圆 上 的 16 个 像素 点 。 
4. 假 如 选取 的 圆 上 有 连续 的 N SB AREA FI, +T FI, -T, AB 


么 像素 p 可 以 被 认为 是 特征 点 (N 通常 取 12， 即 为 FAST-12。 其 他 常用 的 
N 取 值 为 9 和 11， 它 们 分 别 被 称 为 FAST-9 和 FAST-11) o 


5. 循 环 以 上 四 步 ， 对 每 一 个 像素 执行 相同 的 操作 。 


在 FAST12 算 法 中 ， 为 了 更 高 效 ， 可 以 添加 一 项 预测 试 操 作 ， 以 快速 
地 排除 绝 大 多 数 不 是 角 点 的 像素 。 具 体操 作为 ， 对 于 每 个 像素 ， 直 接 检 
测 邻 域 贺 上 的 第 1,5,9,13 个 像素 的 亮度 。 只 有 当 这 4 个 像素 中 有 3 个 同时 大 
FL +T 或 小 于 1, -T 时 ， 当 前 像素 才 有 可 能 是 一 个 角 点 ， 否 则 应 该 直接 排 
除 。 这 样 的 预测 试 操作 大 大 加 速 了 角 点 检测 。 此 外 ， 原 始 的 FAST 角 点 经 
常 出 现 * 扎 堆 ” 的 现象 。 所 以 在 第 一 遍 检 测 之 后 ， 还 需要 用 非 极 大 值 抑制 

(Non-maximal suppression) ， 在 一 定 区 域内 仅 保 留 响应 极 大 值 的 角 点 ， 
避免 角 点 集中 的 问题 。 


图 7-2 FAST 特征 点 3 。 


FAST 特 征 点 的 计算 仅仅 是 比较 像素 间 亮 度 的 差异 ， 速 度 非 常 快 ， 但 
它 也 有 一 些 问 题 。 首 先 ，FAST 特 征 点 数量 很 大 且 不 确定 ， 而 我 们 往往 希 
望 对 图 像 提 取 固 定数 量 的 特征 。 因 此 ， 在 ORB 中 对 原始 的 FAST 算 法 进行 
了 改进 。 我 们 可 以 指定 最 终 要 提取 的 角 点 数量 N ， 对 原始 FAST 角 点 分 别 
计算 Harris 响 应 值 ， 然 后 选取 前 N 个 具有 最 大 响应 值 的 角 点 作为 最 终 的 角 
点 集合 。 

其 次 ，FAST 角 点 不 具有 方向 信息 。 而 且 ， 由 于 它 固定 取 半 径 为 3 的 
圆 ， 存 在 尺度 问题 : 远 处 看 着 像 是 角 点 的 地 方 ， 接 近 后 看 可 能 就 不 是 角 
点 了 。 针 对 FAST 角 点 不 具有 方向 性 和 尺度 的 弱点 ，ORB 添 加 了 尺度 和 旋 
转 的 描述 。 尺 度 不 变性 由 构建 图 像 金 字 塔 让 ， 并 在 金字 塔 的 每 一 层 上 检 
测 角 点 来 实现 。 而 特征 的 旋转 是 由 灰 度 质心 法 (Intensity Centroid) 实现 
的 。 我 们 稍微 介绍 一 下 。 

所 谓 质心 是 指 以 图 像 块 灰 度 值 作为 权重 的 中 心 。 其 具体 操作 步骤 如 
“RES : K 


1. 在 一 个 小 的 图 像 块 B 中 ， 定 义 图 像 块 的 窍 为 


= 》 ay'T(x,y), p,q = {0,1}. 
X,YyEB 


2. 通 过 和 矩 可 以 找到 图 像 块 的 质心 : 


mMıo Moi 
C= (—, —). 
Moo Moo 


3. 连 接 图 像 块 的 几何 中 心 O 与 质心 C ， 得 到 一 个 方向 向 量 5&， 于 是 
特征 点 的 方向 可 以 定义 为 


0= arctan(mo1/m10). 


通过 以 上 方法 ，FAST 角 点 便 具有 了 尺度 与 旋转 的 描述 ， 从 而 大 大 提 
升 了 其 在 不 同 图 像 之 间 表 述 的 健壮 性 。 所 以 在 ORB 中 ， 把 这 种 改进 后 的 
FAST 称 为 Oriented FAST。 


BRIEF 描 述 子 


在 提取 Oriented FAST 关 键 点 后 ， 我 们 对 每 个 点 计算 其 描述 子 。ORB 
使 用 改进 的 BRIEF 特 征 描述 。 我 们 先 来 介绍 一 下 BRIEF 是 什么 。 


BRIEF 是 一 种 二 进 制 描述 子 ， 其 描述 向 量 由 许多 个 0 和 1 组 成 ， 这 里 
的 0 和 1 编码 了 天 键 点 附近 两 个 像素 〈 比 如 p Mq) 的 大 小 关系 : 如 果 p 比 q 
大 ， 则 取 1， 反 之 就 取 0。 如 果 我 们 取 了 128 个 这 样 的 p,q ， 最 后 就 得 到 128 
维 由 0、1 组 成 的 向 量 。 那 么 ，p 和 gq 如 何 选 取 呢 ? 在 作者 原始 的 论文 中 给 
出 了 若干 种 挑选 方法 ， 大 体 上 都 是 按照 某 种 概率 分 布 ， 随 机 地 挑选 p Mq 
的 位 置 ， 读 者 可 以 阅读 BRIEF 论 文 或 OpenCV 源 码 以 查看 其 具体 实现 5 。 
BRIEF 使 用 了 随机 选 点 的 比较 ， 速 度 非常 快 ， 而 且 由 于 使 用 了 二 进 制 表 
达 ， 存 储 起 来 也 十 分 方便 ， 和 运用 于 实时 的 图 像 匹 配 。 原 始 的 BRIEF 描 述 子 
不 具有 旋转 不 变性 ， 因 此 在 图 像 发 生 旋转 时 容易 丢失 。 而 ORB 在 FAST 特 
征 点 提取 阶段 计算 了 关键 点 的 方向 ， 所 以 可 以 利用 方向 信息 ， 计 算 了 旋 
转 之 后 的 “Steer BRIEF” 特 征 使 ORB 的 描述 子 具有 较 好 的 旋转 不 变性 。 


由 于 考虑 到 了 旋转 和 缩放 ， 使 得 ORB 在 平移 、 旋 转 和 缩放 的 变换 下 
仍 有 良好 的 表现 。 同 时 ，FAST 和 BREIF 的 组 合 也 非常 高 效 ， 使 得 ORB 特 
征 在 实时 SLAM 中 非常 受 欢 迎 。 我 们 在 图 7-3 中 展示 了 一 张 图 像 提 取 ORB 
之 后 的 结果 ， 下 面 来 介绍 如 何在 不 同 的 图 像 之 间 进 行 特征 匹配 。 


图 7-3 ”OpenCV 提 供 的 ORB 特 征 点 检测 结果 。 


特征 匹配 (如 图 7-4 所 示 ) 是 视觉 SLAM 中 极为 关键 的 一 步 ， 宽 泛 地 
说 ， 特 征 匹 配 解决 了 SLAM 中 的 数据 关联 问题 (dataassociation) ， 即 确 
定 当 前 看 到 的 路 标 与 之 前 看 到 的 路 标 之 间 的 对 应 关系 。 通 过 对 图 像 与 图 
像 或 者 图 像 与 地 图 之 间 的 描述 子 进行 准确 匹配 ， 我 们 可 以 为 后 续 的 姿态 
估计 、 优 化 等 操作 减轻 大 量 负担 。 然 而 ， 由 于 图 像 特 征 的 局 部 特性 ， 误 
匹配 的 情况 广泛 存在 ， 而 且 长 期 以 来 一 直 没 有 得 到 有 效 解决 ， 目 前 已 经 
成 为 视觉 SLAM 中 制约 性 能 提升 的 一 大 瓶颈 。 部 分 原因 是 场景 中 经 常 存在 
大 量 的 重复 纹理 ， 使 得 特征 描述 非常 相似 。 在 这 种 情况 下 ， 仪 利用 局 部 
特征 解决 误 匹 配 是 非常 困难 的 。 


图 7-4 ”两 帧 图 像 间 的 特征 匹配 。 


不 过 ， 让 我 们 先 来 看 正确 匹配 的 情况 ， 等 做 完 实验 再 回头 去 讨论 误 
匹配 问题 。 考 虑 两 个 时 刻 的 图 像 。 如 果 在 图 像 虐 中 提取 到 特征 点 
mm =1,2,..,M, TERRI, ,, 中 提取 到 特征 点 z%1,n=1,2,.…,N， 如 何 寻 找 这 
两 个 集合 元 素 的 对 应 关系 呢 ? 最 简单 的 特征 匹配 方法 就 是 暴力 匹配 ( 
Brute-Force Matcher) 。 即 对 每 一 个 特征 点 z” 与 所 有 的 x% 测量 描述 子 的 
距离 ， 然 后 排序 ， 取 最 近 的 一 个 作为 匹配 点 。 描 述 子 距离 表示 了 两 个 特 
征 之 间 的 相似 程度 ， 不 过 在 实际 运用 中 还 可 以 取 不 同 的 距离 度量 范 数 。 
对 于 浮 点 类 型 的 描述 子 ， 使 用 欧 氏 距离 进行 度量 即 可 。 而 对 于 二 进 制 的 
描述 子 (比如 BRIEF 这 样 的 ) ， 我 们 往往 使 用 汉 明 距离 (Hamming 
distance) 作为 度量 一 一 两 个 二 进 制 串 之 间 的 汉 明 距离 ， 指 的 是 其 不 同位 
数 的 个 数 。 

然而 ， 当 特征 点 数量 很 大 时 ， 暴 力 匹 配 法 的 运算 量 将 变 得 很 大 ， 特 
别 是 当 想 要 匹配 某 个 帧 和 一 张 地 图 的 时 候 。 这 不 符合 我 们 在 SLAM 中 的 实 
时 性 需求 。 此 时 快速 近似 最 近邻 (FLANN) 算法 更 加 适合 于 匹配 点 数量 
极 多 的 情况 。 由 于 这 些 匹配 算法 理论 已 经 成 熟 ， 而 且 实 现 上 也 已 集成 到 
OpenCV， 所 以 这 里 就 不 再 描述 它 的 技术 细节 了 。 感 兴趣 的 读者 可 以 参考 
阅读 文献 [36]。 


7.2 KR: 特征 提取 和 匹配 


图 7-5 ”实验 使 用 的 两 帧 图 像 。 


目前 主流 的 几 种 图 像 特征 在 OpenCV 开 源 图 像 库 中 都 已 经 集成 ， 我 们 
可 以 很 方便 地 进行 调用 。 下 面 就 来 实际 练习 一 下 OpenCV 的 图 像 特征 提 


取 、 计 算 和 匹配 的 过 程 。 我 们 为 此 实验 准备 了 两 张 图 像 ， 位 于 
slambook/ch7/ 下 的 1.png 和 2.png， 如 图 7-5 所 示 。 它 们 是 来 自 公 开 数 据 集 
[37] 中 的 两 张 图 像 ， 我 们 看 到 相机 发 生 了 微小 的 运动 。 本 节 程 序 演示 如 何 
提取 ORB 特 征 并 进行 匹配 。 下 一 个 程序 将 演示 如 何 估 计 相 机 和 运动。 

特征 提取 与 匹配 代码 : 


内 slambook/ch7/feature_extraction.cpp 


#include <iostream> 

#include <opencv2/core/core.hpp> 

#include <opencv2/features2d/features2d.hpp> 
#include <opencv2/highgui/highgui .hpp> 


wn eS w wv 一 


using namespace std; 


using namespace cv; 


int main ( int argc, char** argv ) 

10 |{ 

11 if ( argc != 3 ) 

12 { 

13 cout<<"usage: feature_extraction imgl img2"<<endl; 
14 return 1; 

15 } 

16 //-- 读 取 图 像 


Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR ) ; 
Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR ) ; 


//-- 初始 化 

std::vector<KeyPoint> keypoints_1, keypoints_2; 

Mat descriptors_1, descriptors_2; 

Ptr<ORB> orb = ORB::create ( 500, 1.2f, 8, 31, 0, 2, ORB::HARRIS_SCORE,31,20 ); 


//-- 第 1 步 : 检测 Oriented FAST 角 点 位 置 
orb->detect ( img_1,keypoints_1 ); 
orb->detect ( img_2,keypoints_2 ); 


//-- 第 2 步 : 根据 角 点 位 置 计 算 BRIEF 描述 子 
orb->compute ( img_1, keypoints_1, descriptors_1 ); 


orb->compute ( img_2, keypoints_2, descriptors_2 ); 


Mat outimg1; 

drawKeypoints( img_1, keypoints_1, outimgi, Scalar::all(-1), DrawMatchesFlags:: 
DEFAULT ); 

imshow ("ORB4¥ 4E 4", outimg1) ; 


//-- 第 3 步 37S ARP t BRIEF i£ F EAT PL Ae, 4È Hamming 距离 
vector<DMatch> matches; 
BFMatcher matcher ( NORM_HAMMING ); 


matcher.match ( descriptors_1, descriptors_2, matches ); 


//-- 第 4 步 : 匹配 点 对 筛选 
double min_dist=10000, max_dist=0; 


// 找 出 所 有 匹配 之 间 的 最 小 距离 和 最 大 距离 ， 即 最 相似 的 和 最 不 相似 的 两 组 点 之 间 的 距离 
for ( int i = 0; i < descriptors_1.rows; i++ ) 
{ 

double dist = matches [i] .distance; 

if ( dist < min_dist ) min dist = dist; 

if ( dist > max_dist ) max_dist = dist; 


printf ( "-- Max dist : %f \n", max_dist ); 
printf ( "-- Min dist : %f \n", min_dist ); 


// 当 描 述 子 之 间 的 距离 大 于 两 倍 的 最 小 距离 时 ， 即 认为 匹配 有 误 
// 但 有 时 候 最 小 距离 会 非常 小 ， 设 置 一 个 经 验 值 作为 下 限 。 

std: :Vector< DMatch > good_matches; 

for ( int i = 0; i < descriptors_1.rows; i++ ) 


{ 


61 if ( matches[i].distance <= max ( 2*min_dist, 30.0 ) ) 


62 { 

63 good_matches.push_back ( matches[i] ); 

64 } 

65 } 

66 

67 //-- 第 5 步 : 绘制 匹配 结果 

68 Mat img_match ; 

69 Mat img_goodmatch; 

70 drawMatches ( img_1, keypoints_1, img_2, keypoints_2, matches, img_match ); 

71 drawMatches ( img_1, keypoints_1, img_2, keypoints_2, good_matches, 
img_goodmatch ); 

72 imshow ( "所 有 匹配 点 对 "，img_match ); 

73 imshow ( "优化 后 匹配 点 对 "，img_goodmatch ) ; 

74 waitKey (0) ; 

75 

76 return 0; 

77 } 


运行 此 程序 〈 需 要 输入 两 个 图 像 位 置 ) ， 将 输出 运行 结果 : 


1 |% build/feature_extraction 1.png 2.png 
2 |-- Max dist : 95.000000 
3 |-- Min dist : 4.000000 
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工程 上 的 经 验方 法 ， 不 一 定 有 理论 依据 。 不 过 ， 尽 管 在 示例 图 像 中 能 够 
ee ee ee ee ee 
都 是 正确 的 。 因 此 ， 在 后 面 的 运动 估计 中 ， 还 需要 使 用 去 除 误 匹 配 的 算 
法 。 


接 下 来 ， 我 们 希望 根据 匹配 的 点 对 估计 相机 的 运动 。 这 里 由 于 相机 
的 原理 不 同 ， 情 况 发 生 了 变化 : 


1. 当 相机 为 单 目 时 ， 我 们 只 知道 2D 的 像素 坐标 ， 因 而 问题 是 根据 两 
组 2Dra 估计 运动 。 该 问题 用 对 极 几何 来 解决 。 


2. 当 相机 为 双 目 、RGB-D 时 ,或 者 通过 某 种 方法 得 到 了 距离 信息 ， 那 
么 问题 就 是 根据 两 组 DA 估计 运动 。 该 问题 通常 用 ICP 来 解决 。 

3. 如 果 有 3D 点 及 其 在 相机 的 投影 位 置 ， 也 能 估计 相机 的 运动 。 该 问 
题 通过 PnP 求解 。 

因此 ， 下 面 几 节 融 来 介绍 这 三 种 情形 下 的 相机 和 运动 估计 。 我 们 将 从 
最 基本 的 2D-2D 情 形 出 发 ， 看 看 它 如 何 求 解 ， 求 解 过 程 又 具有 哪些 麻烦 的 


问题 。 


ORB 关 键 点 


图 7-6 ”特征 提取 与 匹配 结果 。 


7.3 2D-2D: 对 极 几 何 
7.3.1 ”对 极 约束 


现在 ,假设 我 们 从 两 张 图 像 中 得 到 了 一 对 配对 好 的 特征 点 ， 如 图 7-7 
所 示 。 如 果 有 若干 对 这 样 的 匹配 点 ， 就 可 以 通过 这 些 二 维 图 像 点 的 对 应 
天 系 ， 恢 复出 在 两 帧 之 间 摄 像 机 的 运动 。 这 里 “各 干 对 ?具体 是 多 少 对 
呢 ? 我 们 会 在 下 文 介绍 。 下 面 先 来 看 看 两 个 图 像 当 中 的 匹配 点 有 什么 几 
何 关系 。 


图 7-7 对 极 几何 约束 。 


以 图 7-7 为 例 ， 我 们 希望 求 取 两 帧 图 像 1, ,7 , 之 间 的 运动 ， 设 第 一 帧 到 
第 二 帧 的 运动 为 R,t 。 两 个 相机 中 心 分 别 为 0 , ,0 ,。 现 在 ， 考 虑 1 中 有 一 
个 特征 点 p o CEI, 中 对 应 着 特征 点 p ,。 我 们 知道 两 者 是 通过 特征 匹配 
得 到 的 。 如 果 匹 配 正 确 ， 说 明 它 们 确实 是 同一 个 空间 点 在 两 个 成 像 平面 
上 的 投影 。 这 里 需要 一 些 术语 来 描述 它们 之 间 的 几何 关系 。 首 先 ， 连 线 
Di 和 连 线 Op 在 三 维 空间 中 会 相交 于 点 P 。 这 时 候 点 0 , ,0 , P 三 个 点 
可 以 确定 一 个 平面 ， 称 为 极 平 面 (Epipolar plane) -o OO, 连 线 与 像 平 
E, ,1 , 的 交点 分 别 为 e , ,e ,。e 1 ,e , 称 为 极点 (Epipoles) , 0,0, ® 
称 为 基线 (Baseline) 。 我 们 称 极 平面 与 两 个 像 平 面 1, ,1, 之 间 的 相交 线 
1 >! ,为 极 线 (Epipolar line) o 


直观 讲 ， 从 第 一 帧 的 角度 看 ， 射 线 5 志 是 某 个 像素 可 能 出 现 的 空间 
位 置 “因为 该 射线 上 的 所 有 点 都 会 投影 到 同一 个 像素 点 。 同 时 ， 如 果 
不 知道 P 的 位 置 ， 那 么 当 我 们 在 第 二 幅 图 像 上 看 时 ， 连 线 二 上 ( 也 就 是 第 
二 幅 图 像 中 的 极 线 ) 就 是 P 可 能 出 现 的 投影 的 位 置 ， 也 就 是 射线 可 7 在 
第 二 个 相机 中 的 投影 。 现 在 ， 由 于 我 们 通过 特征 点 匹配 确定 了 p ,的 像素 
位 置 ， 所 以 能 够 推断 P 的 空间 位 置 ， 以 及 相机 的 运动 。 要 提醒 读者 的 是 ， 
这 完全 多 亏 了 正确 的 特征 匹配 。 如 果 没有 特征 匹配 ， 我 们 就 没 法 确定 p ， 
到 底 在 极 线 的 哪个 位 置 了 。 那 时 ， 就 必须 在 极 线 上 搜索 以 获得 正确 的 匹 
配 ， 这 将 在 第 13 讲 中 提 到 。 


现在 ， 我 们 从 代数 角度 来 看 一 下 这 里 出 现 的 几何 关系 。 在 第 一 帧 的 
坐标 系 下 ， 设 P 的 空间 位 置 为 


P = [X,Y,2].. 
根据 第 5 讲 介 绍 的 针 孔 相机 模型 ， 我 们 知道 两 个 像素 点 p , ,p , 的 像素 
位 置 为 
sıpı = KP, szp = K(RP +t). (7.1) 


这 里 K 为 相机 内 参 和 矩阵 ，R,t 为 两 个 坐标 系 的 相机 运动 (如果 我 们 愿 
意 ， 也 可 以 写成 李 代 数 形式 ) 。 如 果 使 用 齐 次 坐标 ， 也 可 以 把 上 式 写 成 
在 乘 以 非 零 常数 下 成 立 的 (up to a scale) ERO : 


pi=KP, p=K(RP +t). (7.2) 
现在 ， 取 : 
2,=K'p,, z= Ktp. (7.3) 
这 里 的 x |x, 是 两 个 像素 点 的 归 一 化 平面 上 的 坐标 。 代 入 上 式 ， 得: 
xo = Ra, +t. (7.4) 


PRIA ASE. VIZ “的 定义 ， 这 相当 于 两 侧 同 时 与 5 做 外 积 : 


ta, = t^ Ra. (7.5) 
然后 ， 两 侧 同 时 左 乘 : 
ZJt ro 二 eit Rz. (7.6) 


观察 等 式 左 侧 ，t^x , 是 一 个 与 + 和 x , 都 垂直 的 向 量 。 把 它 再 和 x , 做 
内 积 时 ， 将 得 到 0。 因 此 ， 我 们 就 得 到 了 一 个 简洁 的 式 子 : 


zit* Ra, = 0. (7.7) 


重新 代入 p , ,p , A: 


p,K 't\RK~'p, = 0. (7.8) 

这 两 个 式 子 都 称 为 对 极 约束 ， 它 以 形式 简洁 著名 。 它 的 几何 意义 是 
O ,,PO ,三 者 共 面 。 对 极 约束 中 同时 包含 了 平移 和 旋转 。 我 们 把 中 间 部 
分 记 作 两 个 和 矩阵: 基础 矩阵 (Fundamental Matrix) F ll ZN M FB BE 

(Essential Matrix) EE ， 于 是 可 以 进一步 简化 对 极 约束 : 
E=t\R, F=K "TEK !, «x,Ex, = pl Fp = 0. (7.9) 

对 极 约束 简洁 地 给 出 了 两 个 匹配 点 的 空间 位 置 关 系 。 于 是 ， 相 机 位 
次 估计 问题 变 为 以 下 两 步 : 

1. 根 据 配 对 点 的 像素 位 置 求 出 E RAF o 

2. 根 据 E REF 求 出 R,t o 

AFE 和 F 只 相差 了 相机 内 参 ， 而 内 参 在 SLAM 中 通常 是 已 知 的 中 ， 


所 以 实践 当中 往往 使 用 形式 更 简单 的 E 。 我 们 以 E 为 例 ， 介 绍 上 面 两 个 问 
题 如 何 求解 。 


7.3.2 本质 和 矩阵 


根据 定义 ， 本 质 和 矩阵 已 =tR。 它 是 一 个 3x 3 的 矩阵 ， 内 有 9 个 未 知 
效 。 那 么 ， 是 不 是 任意 一 个 3x 3 的 矩阵 都 可 以 被 当成 本 质 矩 阵 呢 ?从 E 的 
构造 方式 上 看 ， 有 以 下 值得 注意 的 地 方 : 


“本 质 矩 阵 是 由 对 极 约束 定义 的 。 由 于 对 极 约束 是 等 式 为 零 的 约束 ， 
所 以 对 E 乘 以 任意 非 零 常数 后 ， 对 极 约束 依然 满足 。 我 们 把 这 件 事情 称 
AE 在 不 同 尺度 下 是 等 价 的 。 

"根据 E =t^ R ， 可 以 证 明 外 ， 本 质 和 矩阵 E 的 奇异 值 必定 是 [0,0, 0T 的 
形式 。 这 称 为 本 质 和 矩阵 的 内 在 性 质 。 

* 另 一 方面 ， 由 于 平移 和 旋转 各 有 3 个 自由 度 ， 故 t^ R 共有 6 个 自由 
度 。 但 由 于 尺度 等 价 性 ， 故 E 实际 上 有 5 个 自由 度 。 

E 具 有 5 个 自由 度 的 事实 ， 表 明 我 们 最 少 可 以 用 5 对 点 来 求解 E 。 但 


是 , EE 的 内 在 性 质 是 一 种 非 线 性 性 质 ， 在 求解 线性 方程 时 会 珊 来 麻烦 ， 
此 ， 也 可 以 只 考虑 它 的 尺度 等 价 性 ， 使 用 8 对 点 来 估计 E 一 一 这 就 是 经 典 


的 八 点 法 ( Eight-point-algorithm) 839 。 八 点 法 只 利用 了 E 的 线性 性 
质 ， 因 此 可 以 在 线性 代数 框架 下 求解 。 下 面 我 们 来 看 八 点 法 是 如 何 工作 
的 。 


考虑 一 对 匹配 点 ， 它 们 的 归 一 化 坐标 为 x ,=[u yy,, xuv, 
1]* 。 根 据 对 极 约束 ， 有 : 


cl1 €2 €3 U2 
(wi,01,1) e4 65 e v2 | =0. (7.10) 
e7 €8 6E9 1 


我 们 把 矩阵 E 展开 ， 写 成 向 量 的 形式 : 


= T 
e = [ei, €2, €3,€4,€5, €6, €7, €8, €g] ’ 


那么 对 极 约束 可 以 写成 与 e 有 关 的 线性 形式 : 
[wi U2, U1V2, U1, V1U2, V1V2, V1, U2, V2, 1] - e = 0. (7.11) 


同 理 ， 对 于 其 他 点 对 也 有 相同 的 表示 。 我 们 把 所 有 点 都 放 到 一 个 方 
程 中 ， 变 成 线性 方程 组 (ui ,vi 表示 第 i 个 特征 点 ， 以 此 类 推 ) : 


€i 
€2 
Ligh i od eet piled”, Yee cel: lll “a 
UU2 Uv Uz WU VV v1 uz vz 1 
€4 
užu? uo? u? of vive v? uz v2 1 
es | =0. (7.12) 
e6 
uus uos u vus vvs vi uf v 1 
e7 
€g 


这 8 个 方程 构成 了 一 个 线性 方程 组 。 它 的 系数 矩阵 由 特征 点 位 置 构 
成 ， 大 小 为 8x 9。e 位 于 该 矩 阵 的 零 空间 中 。 如 果 系数 和 矩阵 是 满 秩 的 (BM 
秩 为 8) ， 那 么 它 的 零 空 间 维 数 为 1， 也 就 是 e 构成 一 条 线 。 这 与 e 的 尺度 
等 价 性 是 一 致 的 。 如 果 8 对 匹配 点 组 成 的 矩阵 满足 秩 为 8 的 条 件 ， 那 么 E 的 
各 元 素 就 可 由 上 述 方程 解 得 。 

接 下 来 的 问题 是 如 何 根 据 已 经 估 得 的 本 质 和 矩阵 E ， 恢 复出 相机 的 运动 
R,t 。 这 个 过 程 是 由 奇异 值 分 解 (SVD) 得 到 的 。 设 E 的 SVD 分 解 为 


E=UXvV', (7.13) 


其 中 LVY 为 正 交 阵 ，2z 为 奇异 值 和 矩阵 。 根 据 忆 的 内 在 性 质 ， 我 们 知道 
2=diag(aoa, 0)。 在 SVD 分 解 中 ， 对 于 任意 一 个 下 ， 存 在 两 个 可 能 的 5R 与 
它 对 应 : 

tî =URz(4)=U', R, =UR}(E)V" 
tô =URz(-3)5U', R,=UR}(-3)V". 


(7.14) 


其 中 Rz(3) 表示 沿 Z 轴 旋 转 90 " 得 到 的 旋转 矩阵 。 同 时 ， 由 于 -E ME 
等 价 ， 所 以 对 任意 一 个 t 取 负 号 ， 也 会 得 到 同样 的 结果 。 因 此 ， 从 E 分 解 
到 t,R 时 ， 一 共存 在 4 个 可 能 的 解 。 


图 7-8 形 象 地 展示 了 分 解 本 质 和 矩阵 得 到 的 4 个 解 。 我 们 已 知 空间 点 在 相 
机 EE) 上 的 投影 (红色 点 ) ， 想 要 求解 相机 的 运动 。 在 保持 红色 
点 不 变 的 情况 下 ， 可 以 画 出 4 种 可 能 的 情况 。 不 过 幸运 的 是 ， 只 有 第 一 种 
解 中 P 在 两 个 相机 中 都 具有 正 的 深度 。 因 此 ， 只 要 把 任意 一 点 代入 4 种 解 
中 ， 检 测 该 点 在 两 个 相机 下 的 深度 ， 就 可 以 确定 哪个 解 是 正确 的 了 。 


(1) (2) (3) (4) 


图 7-8 DERAN RR ER) 不 变 的 情况 下 ， 两 个 相机 及 空 
间 点 一 共有 4 种 可 能 的 情况 。 


如 果 利用 E 的 内 在 性 质 ， 那 么 它 只 有 5 个 自由 度 。 所 以 最 小 可 以 通过 5 
对 点 来 求解 相机 运动 中 入 。 然 而 这 种 做 法 形式 复杂 ， 从 工程 实现 角度 考 
虑 ， 由 于 平时 通常 会 有 几 十 对 乃至 上 百 对 的 匹配 点 ， 从 8 对 减 至 5 对 意义 
并 不 明显 。 为 保持 简单 ， 我 们 这 里 就 只 介绍 基本 的 八 点 法 。 

剩 下 的 问题 还 有 一 个 : 根据 线性 方程 解 出 的 E ， 可 能 不 满足 E 的 内 在 
性 质 一 一 它 的 奇异 值 不 一 定 为 0,0, 0 的 形式 。 这 时 ， 在 做 SVD 时 我 们 会 刻 
意 地 把 > 矩阵 调整 成 上 面 的 样子 。 通 常 的 做 法 是 ， 对 八 点 法 求 得 的 E 进行 
SVD 分 解 后 ， 会 得 到 奇异 值 矩 阵 >=diag(o , ,0 , ,0  )， 不 妨 设 o , >0 , >0 ,。 
取 : 


01 +02 O1 +02 
2 》 


这 相当 于 是 把 求 出 来 的 矩阵 投影 到 了 E 所 在 的 流 形 上 。 当 然 ， 更 简单 
的 做 法 是 将 奇异 值 和 矩阵 取 成 diag(1, 1 0)， 因 为 E 具有 尺度 等 价 性 ， 所 以 这 
样 做 也 是 合理 的 。 


7.3.3” 单 应 和 矩阵 


除了 基本 矩阵 和 本 质 和 矩 阵 ， 还 有 单 应 矩阵 (Homography) H, CHE 
述 了 两 个 平面 之 间 的 映射 关系 。 若 场景 中 的 特征 点 都 落 在 同一 平面 上 
(比如 墙 、 地 面 等 ) ， 则 可 以 通过 单 应 性 来 进行 运动 估计 。 这 种 情况 在 
无 人 机 携带 的 俯视 相机 或 扫地 机 携带 的 顶 视 相 机 中 比较 常见 。 由 于 之 前 
没有 提 到 过 单 应 ， 因 此 这 里 稍微 介绍 一 下 。 

单 应 矩阵 通常 描述 处 于 共同 平面 上 的 一 些 点 在 两 张 图 像 之 间 的 变换 
KA SRE AI, 有 一 对 匹配 好 的 特征 点 p , 和 p , 。 这 些 特征 点 落 
在 平面 P 上 ， 设 这 个 平面 满足 方程 : 


n'P+id=O0O. (7.16) 


稍 加 整理 ， 得 : 


E = Udiag( 0) VT. (7.15) 


= 1. (7.17) 


然后 ， 回 顾 本 节 开 头 的 式 (7.1)， 得 : 


于 是 ， 我 们 得 到 了 一 个 直接 描述 图 像 坐标 p , Mp, 之 间 的 变换 ， 把 中 
间 这 部 分 记 为 万 ， 于 是 : 
p2 = Hpi. (7.18) 


它 的 定义 与 旋转 、 平 移 及 平面 的 参数 有 关 。 与 基础 矩阵 F 类 似 ， 单 应 
窍 阵 五 也 是 一 个 3x 3 的 和 矩阵， 求解 时 的 思路 也 和 FF 类似， 同样 可 以 先 根 气 
匹配 点 计算 万 ， 然 后 将 它 分 解 以 计算 旋转 和 平移 。 把 上 式 展开 ， 得 : 


U2 hi ho hs Ul 
V2 = h4 hs he V1 . (7. 19) 
1 hz hg hg 1 


请 注意 ， 这 里 的 等 号 是 在 非 零 因子 下 成 立 的 。 我 们 在 实际 处 理 中 通 
常 乘 以 一 个 非 零 因子 使 得 h , =1 (在 它 取 非 零 值 时 ， 。 然 后 根据 第 3 行 ， 去 
掉 这 个 非 零 因 子 ， 于 是 有 : 

ia hiui + havı + hg 

A hyu + hgv1 十 ho 

7 hau, + hsv + he 
i hzu, + hgv; + ho : 


U2 


整理 得 : 
hiui + həvı + hg == h7u ug = hgv uz = U2 


hauy + hsv1 十 he = h7u1v2 = hgv v2 = U2. 


这 样 一 组 匹配 点 对 就 可 以 构造 出 两 项 约束 (事实 上 有 三 个 约束 ， 但 
是 因为 线性 相关 ， 只 取 前 两 个 ) ， 于 是 自由 度 为 8 的 单 应 矩阵 可 以 通过 4 
对 匹配 特征 点 算出 (注意 ， 这 些 特征 点 不 能 有 三 点 共 线 的 情况 ) ， 即 求 
解 以 下 的 线性 方程 组 ( 当 h ,=0 时 ， 右 侧 为 零 ) : 


ul vi 1 0 0 0 -ulud —vlul hi us 
0 0 0 ut vt 1 uted —v} vd hə vs 
u? v? 1 0 0 0 —uĝĵu? —v?u2 hs u2 
0 0 0 vu? v? 1 uv? —v?u2 h4 v2 
3 3 33 a ma 
“a vy 1 0 0 0 uv Kee hs Ud 
0 0 0 ui v 1 一 03 一 oo3| | he v3 
ut vf 1 0 0 0 —ufus —vfus hr us 
0 0 0 ut vi 1 一 Los 一 oo hg vå 


这 种 做 法 把 互 和 矩阵 看 成 了 向 量 ， 通 过 解 该 向 量 的 线性 方程 来 恢复 五 
， 又 称 直接 线性 变换 法 (Direct Linear Transform) 。 与 本 质 和 矩阵 相似 ， 求 
出 单 应 和 矩阵 以 后 需要 对 其 进行 分 解 ， 才 可 以 得 到 相应 的 旋转 矩阵 R 和 平移 
向 量 ! 。 分 解 的 方法 包括 数值 法 中 4 SRAM 。 与 本 质 和 矩阵 的 分 解 类 
似 ， 单 应 矩阵 的 分 解 同 样 会 返回 4 组 旋转 和 矩阵 与 平移 向 量 ， 并 且 同 时 可 以 
计算 出 它们 分 别 对 应 的 场景 点 所 在 平面 的 法 向 量 。 如 果 已 知 成 像 的 地 图 
点 的 深度 全 为 正 值 〈 即 在 相机 前 方 ) ， 则 又 可 以 排除 两 组 解 。 最 后 仅 剩 
两 组 解 ， 这 时 需要 通过 更 多 的 先 验 信息 进行 判断 。 通 常 我 们 可 以 通过 假 
设 已 知 场景 平面 的 法 向 量 来 解决 ， 如 场景 平面 与 相机 平面 平行 ， 那 么 法 
向 量 n 的 理论 值 为 17 。 


单 应 性 在 SLAM 中 具有 重要 意义 。 当 特征 点 共 面 或 者 相机 发 生 纯 旋转 
时 ， 基 础 矩阵 的 自由 度 下 降 ， 这 就 出 现 了 所 谓 的 退化 (degenerate) 。 现 
实 中 的 数据 总 包含 一 些 噪 声 ， 这 时 候 如 果 继 续 使 用 八 点 法 求解 基础 矩 
阵 ， 基 础 矩阵 多 余 出 来 的 自由 度 将 会 主要 由 噪声 决定 。 为 了 能 够 避免 退 
化 现象 造成 的 影响 ， 通 党 我 们 会 同时 估计 基础 矩阵 F 和 单 应 矩阵 瓦 ， 选 择 
重 投影 误差 比较 小 的 那个 作为 最 终 的 运动 估计 算 阵 。 


7.4 实践 : 对 极 约束 求解 相机 运动 


下 面 ， 我 们 来 练习 一 下 如 何 通 过 本 质 和 矩 阵 求解 相机 运动 。 上 一 节 


践 部 分 的 得 序 提供 了 特征 匹配 ， 而 这 次 我 们 就 使 用 匹配 好 的 特征 点 来 计 


SEF 和 万 ， 进 而 分 解 E 得 到 R,t 。 整 个 程序 使 用 OpenCV 提 供 的 算法 进行 
求解 。 我 们 把 上 一 节 的 特征 提取 封装 成 溯 数 ， 以 供 后 面 使 用 。 本 节 只 展 


示 位 姿 估 计 部 分 的 代码 。 
四 slambook/ch7/pose_estimation_2d2d.cpp (片段 ) 


1 )y 


1 | void pose_estimation_2d2d ( 

2 std::vector<KeyPoint> keypoints_1, 

3 std: :vector<KeyPoint> keypoints_2, 

4 std::vector< DMatch > matches, 

5 Mat& R, Mat& t ) 

6 |{ 

7 // 相机 内 参 ，TUM Freiburg2 

8 Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, O, 
9 

10 //-- 把 匹配 点 转换 为 vector<Point2af> 的 形式 

11 vector<Point2f> points1; 

12 vector<Point2f> points2; 

13 

14 for ( int i = 0; i < ( int ) matches.size(); i++ ) 

15 { 

16 points1.push_back( keypoints_1[matches[i].queryIdx].pt ); 

17 points2.push_back( keypoints_2[matches[i].trainIdx].pt ); 

18 } 

19 

20 //-- 计算 基础 矩阵 

21 Mat fundamental_matrix; 

2 fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT ); 
23 cout<<"fundamental_matrix is "<<endl<< fundamental_matrix<<end1l; 


24 


25 //-- H HARE 


26 Point2d principal_point ( 325.1, 249.7); // #8, TUM dataset 标定 值 
27 int focal_length = 521; // 3E, TUM dataset 标定 值 
28 Mat essential_matrix; 

29 essential_matrix = findEssentialMat ( points1, points2, focal_length, 


principal_point, RANSAC ); 


30 cout<<"essential_matrix is "<<endl<< essential_matrix<<end1l; 

31 

32 //-- 计算 单 应 矩阵 

33 Mat homography_matrix; 

34 homography_matrix = findHomography ( pointsi, points2, RANSAC, 3, noArray(), 
2000, 0.99 ); 

35 cout<<"homography_matrix is "<<endl<<homography_matrix<<end1; 

36 

37 //-- 从 本 质 矩 阵 中 恢复 旋转 和 平移 信息 

38 recoverPose ( essential_matrix, points1, points2, R, t, focal_length, 


principal_point ); 


39 cout<<"R is "<<endl<<R<<endl; 
40 cout<<"t is "<<endl<<t<<endl; 
a |} 


该 函数 提供 了 从 特征 点 求解 相机 运动 的 部 分 ， 然 后 ， 我 们 在 主 函 数 
中 调用 它 ， 就 能 得 到 相机 的 运 


办 slambook/ch7/pose_estimation_2d2d.cpp (片段 ) 


int main( int argc, char** argv ) 


{ 


if ( argc != 3 ) 


{ 
cout<<"usage: feature_extraction imgl img2"<<endl; 
return 1; 

} 

//-- 读 取 图 像 

Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR ) ; 


Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR ) ; 


vector<KeyPoint> keypoints_1, keypoints_2; 
vector<DMatch> matches; 
find_feature_matches( img_1, img _2, keypoints_1, keypoints_2, matches ); 


cout<<"— + 4% 4] J "<<matches.size()<<"2H 2 ft 4."<<endl; 


//-- 估计 两 张 图 像 间 的 运动 
Mat R,t; 
pose_estimation_2d2d( keypoints_1, keypoints_2, matches, R, t ); 


20 

21 //-- 验证 E=t Rescale 

2 Mat t_x = (Mat_<double>(3,3) << 

23 O; -t.at<double>(2,0), t.at<double>(1,0), 
24 t.at<double>(2,0), 0, -t.at<double>(0,0), 
25 -t.at<double>(1.0), t.at<double>(0,0), 0); 

26 

27 cout<<'"t R="<<endl<<t_x*R<<endl; 

28 //-- 验证 对 极 约 束 

29 Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 ); 
30 for ( DMatch m: matches ) 

31 { 

32 Point2d pt1 = pixel2cam( keypoints_1[ m.queryIdx ].pt, K ); 

33 Mat y1 = (Mat_<double>(3,1) << pti.x, pti.y, 1); 

34 Point2d pt2 = pixel2cam( keypoints_2[ m.trainIdx ].pt, K ); 

35 Mat y2 = (Mat_<double>(3,1) << pt2.x, pt2.y, 1); 

36 Mat d = y2.t() * t_x * R * yl; 

37 cout << "epipolar constraint = " << d << endl; 

38 } 

39 return 0; 

4 |} 


FRI RRR HAH T EF AH 的 数值 ， 然 后 验证 了 对 极 约束 是 否 
立 ， 以 及 t^ RADE 在 非 零 数 乘 下 等 价 的 事实 。 现 在 ， 调 用 此 程序 即 可 看 到 
输出 结果 : 


1 |% build/pose_estimation_2d2d 1.png 2.png 

2 |-- Max dist : 95.000000 

3 |-- Min dist : 4.000000 

4 | 一 共 找 到 了 79 组 匹配 点 

5 |fundamental_matrix is 

6 | [4.844484382466111e-06, 0.0001222601840188731, -0.01786737827487386; 
7 | -0.0001174326832719333, 2.122888800459598e-05, -0.01775877156212593; 
s |0.01799658210895528, 0.008143605989020664, 1] 

9 |essential_matrix is 

10 | [-0.0203618550523477, -0.4007110038118445, -0.03324074249824097 ; 

1 | 0.3939270778216369, -0.03506401846698079, 0.5857110303721015; 

12 | -0.006788487241438284, -0.5815434272915686, -0.01438258684486258] 

13 | homography_matrix is 

14 | [0.9497129583105288, -0.143556453147626, 31.20121878625771; 

15 | 0.04154536627445031, 0.9715568969832015, 5.306887618807696; 

16 | -2.81813676978796e-05, 4.353702039810921e-05, 1] 

17 |R is 

is | [(0.9985961798781875, -0.05169917220143662, 0.01152671359827873; 

19 |0.05139607508976055, 0.9983603445075083, 0.02520051547522442; 

2 | -0.01281065954813571, -0.02457271064688495, 0.9996159607036126] 


a |t as 

2 | [-0.8220841067933337 ; 
23 | -0.03269742706405412; 
24 | 0.5684264241053522] 


2% |t^R= 

27 | [0.02879601157010516, 0.5666909361828478, 0.04700950886436416; 
28 | -0.5570970160413605, 0.0495880104673049, -0.8283204827837456 ; 
29 | 0.009600370724838804, 0.8224266019846683, 0.02034004937801349] 
[0 .002528128704106625] 
[-0.001663727901710724] 
[-0.0008009088410884102] 


30 | epipolar constraint 


31 | epipolar constraint 


32 | epipolar constraint 


在 程序 的 输出 结果 可 以 看 出 ， 对 极 约束 的 满足 精度 约 在 10 Eko 
根据 前 面 的 讨论 ， 分 解 得 到 的 Rt 一 共有 4 种 可 能 性 。 不 过 ，OpenCV 会 蔡 
我 们 使 用 三 角 化 检测 角 点 的 深度 是 否 为 正 ， 从 而 选 出 正确 的 解 。 


需要 注意 的 地 方 是 ， 我 们 要 弄 清 程序 求解 出 来 的 Rr 是 什么 意义 。 按 
照例 程 的 定义 ， 我 们 的 对 极 约束 是 从 


zə = Ra,+t 


得 到 的 。 这 里 的 Ri 组 成 的 变换 矩阵 ， 是 第 一 个 图 到 第 二 个 图 的 坐标 
变换 和 矩阵 : 


T2 = Toi £1. (7.21) 


请 读者 在 实践 中 务必 清楚 这 里 使 用 的 变换 顺序 (因为 有 时 我 们 会 用 T 
2 ) ， 它 们 非常 容易 搞 反 。 

讨论 

从 演示 程序 中 可 以 看 到 ， 输 出 的 E 和 F 之 间 相 差 了 相机 内 参 和 矩 阵 。 虽 
然 它们 在 数值 上 并 不 直观 ， 但 可 以 验证 它们 的 数学 关系 。 从 E,F MH Ba) 
以 分 解 出 运动 ， 不 过 互 需要 假设 特征 点 位 于 平面 上 。 对 于 本 实验 的 数 
据 ， 这 个 假设 是 不 好 的 ， 所 以 我 们 这 里 主要 用 E 来 分 解 运动 。 

值得 一 提 的 是 ， 由 于 E 本 身 具有 尺度 等 价 性 ， 它 分 解 得 到 的 t,R 也 有 
一 个 尺度 等 价 性 。 而 RE SO(3) 自 身 具有 约束 ， 所 以 我 们 认为 :具有 一 个 尺 
度 。 换 言 之 ， 在 分 解 过 程 中 ， 对 t 乘 以 任意 非 零 常数 ， 分 解 都 是 成 立 的 。 
因此 ， 我 们 通常 把 t 进行 归 一 化 》 让 它 的 长 度 等 于 1。 

尺度 不 确定 性 


对 t 长度 的 归 一 化 ， 直 接 导致 了 单 目 视觉 的 尺度 不 确定 性 (Scale 
Ambiguity) 。 例 如 ， 程 序 中 输出 的 t 第 一 维 约 为 0.822。 这 个 0.822 究 竟 是 
指 0.822 米 还 是 0.822 厘 米 ， 我 们 是 没 法 确定 的 。 因 为 对 t 乘 以 任意 比例 常 
数 后 ， 对 极 约束 依然 是 成 立 的 。 换 言 之 ， 在 单 目 SLAM 中 ， 对 轨迹 和 地 图 
同时 缩放 任意 倍数 ， 我 们 得 到 的 图 像 依然 是 一 样 的 。 这 在 第 2 讲 中 就 已 经 
向 读者 介绍 过 了 。 


在 单 目 视 觉 中 ， 我 们 对 两 张 图 像 的 ! 归 一 化 相当 于 国定 了 尺度 。 虽然 
我 们 不 知道 它 的 实际 长 度 是 多 少 ， 但 我 们 以 这 时 的 + 为 单位 1， 计 算 相机 
运动 和 特征 点 的 3D 位 置 。 这 被 称 为 单 目 SLAM 的 初始 化 。 在 初始 化 之 
后 ， 就 可 以 用 3D-2D 来 计算 相机 运动 了 。 初 始 化 之 后 的 轨迹 和 地 图 的 单 
位 ， 就 是 初始 化 时 固定 的 尺度 。 因 此 ， 单 目 SLAM 有 一 步 不 可 避免 的 初始 


化 。 初 始 化 的 两 张 图 像 必 须 有 一 定 程 度 的 平移 ， 而 后 的 轨迹 和 地 图 都 将 
以 此 步 的 平移 为 单位 。 

除了 对 t 进行 归 一 化 之 外 ， 另 一 种 方法 是 令 初始 化 时 所 有 的 特征 点 平 
均 深 度 为 1， 也 可 以 固定 一 个 尺度 。 相 比 于 令 t 长 度 为 1 的 做 法 ， 把 特征 点 
深度 归 一 化 可 以 控制 场景 的 规模 大 小 ， 使 计算 在 数值 上 更 稳定 些 。 不 过 
这 并 没有 理论 上 的 差别 。 


初始 化 的 纯 旋 转 问 题 


ME 分 解 到 R,t 的 过 程 中 ， 如 果 相 机 发 生 的 是 纯 旋转 ， 导 致 1 为 零 ， 那 
么 ， 得 到 的 E 也 将 为 零 ， 这 将 导致 我 们 无 从 求解 R 。 不 过 ， 此 时 我 们 可 以 
依靠 太 求 取 旋 转 ， 但 仅 有 旋转 时 ， 我 们 无 法 用 三 角 测 量 估 计 特 征 点 的 空 
间 位 置 (这 将 在 下 文 提 到 ) ， 于 是 ， 另 一 个 结论 是 ， 单 目 初 始 化 不 能 只 
有 纯 旋转 ， 必 须要 有 一 定 程 度 的 平移 。 如 果 没 有 平移 ， 单 目 将 无 法 初始 
化 。 在 实践 当中 ， 如 果 初 始 化 时 平移 太 小 ， 会 使 得 位 资 求解 与 三 角 化 结 
果 不 稳定 ， 从 而 导致 失败 。 相 对 地 ， 如 果 把 相机 左右 移动 而 不 是 原 地 旋 
转 ， 就 容易 让 单 目 SLAM 初 始 化 。 因 而 ， 有 经 验 的 SLAM 人 研究 人 员 ， 在 单 
目 SLAM 情 况 下 经 常 选 择 让 相机 进行 左右 平移 以 顺利 地 进行 初始 化 。 

多 于 8 对 点 的 情况 

当 给 定 的 点 数 多 于 8 对 时 〈 比 如 ， 例 程 找到 了 79 对 匹配 ) ， 我 们 可 以 
计算 一 个 最 小 二 乘 解 。 回 忆 式 (7.12) 中 线性 化 后 的 对 极 约束 ， 我 们 把 左 侧 
的 系数 和 矩 阵 记 为 A : 


4e = 0. (7.22) 


对 于 八 点 法 ，A 的 大 小 为 8x 9。 如 果 给 定 的 匹配 点 多 于 8， 该 方程 构 
成 一 个 超 定 方程 ， 即 不 一 定 存 在 e 使 得 上 式 成 立 。 因 此 ， 可 以 通过 最 小 化 
一 个 二 次 型 来 求 : 


min |4ell2 = mine’ A" Ae. (7.23) 


于 是 就 求 出 了 在 最 小 二 乘 意 义 下 的 正和 矩阵。 不 过 ， 当 可 能 存在 误 匹 配 
的 情况 时 ， 我 们 会 更 倾向 于 使 用 随机 采样 一 致 性 〈 Random Sample 
Concensus， RANSAC) 来 求 ， 而 不 是 最 小 二 乘 。RANSAC 是 一 种 通用 
的 做 法 ， 适 用 于 很 多 带 错误 数据 的 情况 ， 可 以 处 理 带 有 错误 匹配 的 数 
据 。 


75 ”三角 测量 


之 前 两 节 我 们 使 用 对 极 几 何 约束 估计 了 相机 和 运动， 也 讨论 了 这 种 方 
法 的 局 限 性 。 在 得 到 运动 之 后 ， 下 一 步 我 们 需要 用 相机 的 运动 估计 特征 
点 的 空间 位 置 。 在 单 目 SLAM 中 ， 仪 通过 单 张 图 像 无 法 获得 像素 的 深度 信 
息 ， 我 们 需要 通过 三 角 测量 (Triangulation) (或 三 角 化 ) 的 方法 来 估 
计 地 图 点 的 深度 ， 如 图 7-9 所 示 。 


图 7-9 三 角 化 获得 地 图 点 深度 


三 角 测 量 是 指 ， 通 过 在 两 处 观察 同一 个 点 的 夹 角 ， 从 而 确定 该 点 的 
距离 。 三 角 测 量 最 早 由 高 斯 提出 并 应 用 于 测量 学 中 ， 它 在 天 文学 、 地 理 
学 的 测量 中 都 有 应 用 。 例 如 ， 我 们 可 以 通过 不 同 季节 观察 到 的 星星 的 角 
度 ， 估 计 它 离 我 们 的 距离 。 在 SLAM 中 ， 我 们 主要 用 三 角 化 来 估计 像素 点 
的 距离 。 

和 上 一 节 类 似 ， 考 虑 图 像 六 和 T,，， 以 左 图 为 参考 ， 右 图 的 变换 矩阵 
为 T。 相 机 光 心 为 O ,和 O , 。 在 1, 中 有 特征 点 p，， 对 应 7 中 有 特征 点 p ， 
o 理论 上 直线 O , p , 与 0 ,p ,在 场景 中 会 相交 于 一 点 P ， 该 点 即 两 个 特征 
点 所 对 应 的 地 图 点 在 三 维 场景 中 的 位 置 。 然 而 由 于 噪声 的 影响 ， 这 两 条 
直线 往往 无 法 相交 。 因 此 ， 可 以 通过 最 二 小 乘法 求解 。 


按照 对 极 几何 中 的 定义 ， 设 x , ,x , 为 两 个 特征 点 的 归 一 化 坐标 ， 那 么 
它们 满足 : 
sıxı = s2 Rx2 +t. (7.24) 
现在 我 们 已 经 知道 了 R,t ， 想 要 求解 的 是 两 个 特征 点 的 深度 s | ,s , 。 
当然 这 两 个 深度 是 可 以 分 开 求 的 ， 比 如 ， 先 来 看 s , 。 如 果 要 算 s , MA 
先 对 上 式 两 侧 左 乘 一 个 x^ | FF: 


S1T1 TI1 =0= soa) Rx 一 xit. (7.25) 


该 式 左 侧 为 零 ， 右 侧 可 看 成 s , 的 一 个 方程 ， 可 以 根据 它 直接 求 得 s ， 
o ATs, s, CES BZK. FE, RIMIS T AW FEIA AIA 
度 ， 确 定 了 它们 的 空间 坐标 。 当 然 ， 由 于 噪声 的 存在 ， 我 们 估 得 的 Rir 不 
一 定 精确 使 式 (7.24) 为 零 ， 所 以 更 单 见 的 做 法 是 求 最 小 二 乘 解 而 不 是 零 
解 。 


7.6 XBR: 三 角 测 量 
76.1 三 角 测量 代 码 


下 面 ， 我 们 演示 如 何 根 据 之 前 利用 对 极 几 何 求 解 的 相机 位 姿 ， 通 过 
三 角 化 求 出 上 一 节 特 征 点 的 空间 位 置 。 我 们 调用 OpenCV 提 供 的 
triangulation 崩 | 数 进行 三 角 化 。 


内 slambook/ch7/triangulation.cpp (片断 ) 


void triangulation ( 
const vector<KeyPoint>& keypoint_1, 
const vector<KeyPoint>& keypoint_2, 
const std::vector< DMatch >& matches, 
const Mat& R, const Mat& t, 
vector<Point3d>& points 


ae 


void triangulation ( 
const vector< KeyPoint >& keypoint_1i, 
const vector< KeyPoint >& keypoint_2, 
const std::vector< DMatch >& matches, 
const Mat& R, const Mat& t, 
vector< Point3d >& points ) 


Mat T1 = (Mat_<double> (3,4) << 
1,0,0,0, 
0,1,0,0, 
0,0,1,0); 

Mat T2 = (Mat_<double> (3,4) << 
R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), t.at<double>(0,0), 
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), t.at<double>(1,0), 
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), t.at<double>(2,0) 
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26 
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Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 
vector<Point2d> pts_1, pts_2; 
for ( DMatch m:matches ) 


{ 
// 将 像素 坐标 转换 至 相机 坐标 
pts_1.push_back ( pixel2cam( keypoint_1[m.queryIdx].pt, K) ); 
pts_2.push_back ( pixel2cam( keypoint_2[m.trainIdx].pt, K) ); 
} 
Mat pts_4d; 


cv::triangulatePoints( T1, T2, pts_1, pts_2, pts_4d ); 


// 转换 成 非 齐 次 坐标 
for ( int i=0; i<pts_4d.cols; i++ ) 
{ 
Mat x = pts_4d.col(i); 
x /= x.at<float>(3,0); // 归 一 化 
Point3d p ( 
x.at<float>(0,0), 
x.at<float>(1,0), 
x.at<float>(2,0) 
) ; 
points.push_back( p ); 


同时 ， 在 main 了 六 数 中 增加 三 角 测 量 部 分 ， 并 验证 重 投影 关系 : 


1 | int main (int argc, char** argv) 
2 |£ 
3 ri ston 
4 -- 三 角 化 
5 vector<Point3d> points; 
6 triangulation( keypoints_1, keypoints_2, matches, R, t, points ); 
7 
3 //-- 验证 三 角 化 点 与 特征 点 的 重 投影 关系 
9 Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1); 
10 for ( int i=0; i<matches.size(); i++ ) 
1l { 
12 Point2d pti_cam = pixel2cam( keypoints_1[ matches[i].queryIdx ].pt, K ); 
13 Point2d pti_cam_3d ( 
14 points [i] .x/points[i].z, 
15 points [i] .y/points[i].z 
16 J; 
17 
18 cout<<"point in the first camera frame: "<<pt1_cam<<endl; 
19 cout<<"point projected from 3D "<<pt1_cam_3d<<", d="<<points [i] .z<<end1; 
20 
21 // 第 2 幅 图 
22 Point2f pt2_cam = pixel2cam( keypoints_2[ matches[i].trainIdx ].pt, K ); 
23 Mat pt2_trans = R*( Mat_<double>(3,1) << points[i].x, points[i].y, points[i 
J.z) + ty 
24 pt2_trans /= pt2_trans.at<double>(2,0); 
25 cout<<"point in the second camera frame: "<<pt2_cam<<endl; 
26 cout<<"point reprojected from second frame: "<<pt2_trans.t()<<endl; 
27 cout<<endl; 
28 } 
29 TF aan 
30 |} 


RIFE TENE AEAN EIA PRAMS RR EE 


一 一 相当 于 P 的 


役 影 位 置 与 看 到 的 特征 点 位 置 。 由 于 误差 的 存在 ， 它 们 会 


有 一 些微 小 的 差异 。 以 下 是 某 一 特征 点 的 信息 : 


1 |point in the first camera frame: [0.0844072, -0.0734976] 

point projected from 3D [0.0843702, -0.0743606], d=14.9895 

point in the second camera frame: [0.0431343, -0.0459876] 

point reprojected from second frame: [0.04312769812378599, -0.04515455276163744, 1] 


N 


D w 


可 以 看 到 ， 误 差 的 量 级 大 约 在 小 数 点 后 第 3 位 。 可 以 看 到 ， 三 角 化 特 
征 点 的 距离 大 约 为 15。 但 由 于 尺度 不 确定 性 ， 我 们 并 不 知道 这 里 的 15 究 


竟 是 多 少 米 。 


7.6.2 ”讨论 


关于 三 角 测 量 ， 还 有 一 个 必须 注意 的 地 方 。 


三 角 测 量 是 由 平移 得 到 的 ， 有 平移 才 会 有 对 极 几何 中 的 三 角形 ， 才 
谈 得 上 三 角 测量 。 因 此 ， 纯 旋转 是 无 法 使 用 三 角 测 量 的 ， 因 为 对 极 约 束 
将 永远 满足 。 在 平移 存在 的 情况 下 ， 我 们 还 要 关心 三 角 测 量 的 不 确定 
性 ， 这 会 引出 一 个 三 角 测 量 的 矛盾 。 

如 图 7-10 所 示 ， 当 平移 很 小 时 ， 像 素 上 的 不 确定 性 将 导致 较 大 的 深度 
不 确定 性 。 也 就 是 说 ， 如 果 特 征 点 运动 一 个 像素 6x ， 使 得 视线 角 变 化 了 
一 个 角度 50 ， 那 么 将 测量 到 深度 值 有 6d 的 变化 。 从 几何 关系 可 以 看 到 ， 
当 t 较 大 时 ，6d 将 明显 变 小 ， 这 说 明 平移 较 大 时 ， 在 同样 的 相机 分 辨 率 
下 ， 三 角 化 测量 将 更 精确 。 对 该 过 程 的 定量 分 析 可 以 使 用 正弦 定理 得 
到 ， 不 过 这 里 先 考虑 定性 分 析 。 


因此 ， 要 提高 三 角 化 的 精度 ， 其 一 是 提高 特征 点 的 提取 精度 ， 也 误 
是 提高 图 像 分 辨 率 一 一 但 这 会 导致 图 像 变 大 ， 增 加 计算 成 本 。 另 一 方式 
是 使 平移 量 增 大 。 但 是 ， 这 会 导致 图 像 的 外 观 发 生 明 显 的 变化 ， 比 如 箱 
子 原先 被 挡住 的 侧面 显示 出 来 ， 又 比如 反射 光 发 生变 化 ， 等 等 。 外 观 变 
化 会 使 得 特征 提取 与 匹配 变 得 困难 。 总 而 言 之 ， 再 增 大 平移 ， 会 导致 匹 
配 失 效 ; 而 平移 太 小 ， 则 三 角 化 精度 不 够 一 一 这 就 是 三 角 化 的 矛盾 。 


ôd XP 
60 
60 
t 
O, O, O, O, 
t 较 小 ， 不 确定 性 大 t 较 大 ， 不 确定 性 小 


图 7-10 三 角 测量 的 矛盾 。 


里 然 本 节 只 介绍 了 三 角 化 的 深度 估计 ， 但 只 要 我 们 愿意 ， 也 能 够 定 
量 地 计算 每 个 特征 点 的 位 置 及 不 确定 性 。 所 以 ， 如 果 假 设 特征 点 服从 高 
斯 分 布 ， 并 且 不 断 地 对 它 进 行 观 测 ， 在 信息 正确 的 情况 下 ， 我 们 就 能 够 
期 望 它 的 方差 会 不 断 减 小 乃至 收敛 。 这 就 得 到 了 一 个 滤波 器 ， 称 为 深度 
滤波 器 (Depth Filter) 。 不 过 ， 由 于 它 的 原理 较 复杂 ， 我 们 将 留 到 第 13 
讲 再 详细 讨论 它 。 下 面 ， 我 们 来 讨论 从 3D-2D 的 匹配 点 来 估计 相机 运动 ， 
以 及 3D-3D 的 估计 方法 。 


7.7 3D-2D PnP 


PnP (Perspective-n-Point) 是 求解 3D 到 2D 点 对 运动 的 方法 。 它 描述 
了 当知 道 n 个 3D 空 间 点 及 其 投影 位 置 时 ， 如 何 估计 相机 的 位 姿 。 前 面 说 
到 ，2D-2D 的 对 极 几 何方 法 需要 8 个 或 8 个 以 上 的 点 对 (以 八 点 法 为 例 ) ， 
且 存 在 着 初始 化 、 纯 旋转 和 尺度 的 问题 。 然 而 ， 如 果 两 张 图 像 中 其 中 一 
张 特 征 点 的 3D 位 置 已 知 ， 那 么 最 少 只 需 3 个 点 对 (需要 至 少 一 个 额外 点 验 
证 结果 ) 就 可 以 估计 相机 和 运动。 特征 点 的 3D 位 置 可 以 由 三 角 化 或 者 RGB- 
D 相 机 的 深度 图 确定 。 因 此 ， 在 双 目 或 RGB-D 的 视觉 里 程 计 中 ， 我 们 可 以 
直接 使 用 PnP 估计 相机 和 运动。 而 在 单 目 视觉 里 程 计 中 ， 必 须 先 进行 初始 
化 ， 然 后 才能 使 用 PnP。3D-2D 方 法 不 需要 使 用 对 极 约束 ， 又 可 以 在 很 少 
的 匹配 点 中 获得 较 好 的 运动 估计 ， 是 最 重要 的 一 种 姿态 估计 方法 。 


PnP 问 题 有 很 多 种 求解 方法 ， 例 如， 用 3 对 点 估计 位 姿 的 P3P' 写 、 直 
接线 性 变换 (DLT) 、EPnP (E ffi cientPnP) “!, UPnP”! , SS, itt 
外 ， 还 能 用 非 线 性 优化 的 方式 ， 构 建 最 小 二 乘 问题 并 迭代 求解 ， 也 就 是 
万 金 油 式 的 Bundle Adjustment。 我 们 先 来 看 DLT， 然 后 再 讲解 Bundle 
Adjustment. 


7.7.1 ”直接 线性 变换 


考虑 某 个 空间 点 P ， 它 的 齐 次 坐标 为 P =(X,YZ, 1)7 。 在 图 像 1, 中 ， 投 
影 到 特征 点 x ,=(u j vi, 1) 《以 归 一 化 平面 齐 次 坐标 表示 ) 。 此 时 相机 的 
位 姿 R,t 是 未 知 的 。 与 单 应 和 矩 阵 的 求解 类 似 ， 我 们 定义 增 广 矩 阵 [RI ] 为 一 
个 3x 4 的 和 矩阵， 包含 了 旋转 与 平移 信息 由。 我们 将 其 展开 形式 列 写 如 下 : 


X 
Ul t1 ta t3 t4 
Y 
S| vi | = |ts te t7 tg . (7.26) 
Z 
1 to tio ti tie | 


用 最 后 一 行 把 s 消去 ， 得 到 两 个 约束 : 为 了 简化 表示 ， 定 义 T 的 行 向 


tX +teY +t37 + t4 t5 X + tY +t7Z + tg 
ui = = ’ u= ` 
“oK Foy ruat ~ bA Fip +tuZ + try 


tı = (t1, to, tz, ta)", to = (ts, te, t7, tg)", ts = (to, tio, t11; t12)", 
于 是 有 : 
t?P — t! Pu, =0, 
和 


t3P —t3Pv = 0. 


请 注意 ，t 是 待 求 的 变量 ， 可 以 看 到 ， 每 个 特征 点 提供 了 两 个 天 于 { 
的 线性 约束 。 假 设 一 共有 NN 个 特征 点 ， 则 可 以 列 出 如 下 线性 方程 组 : 


Pi 0 uP? 

0 PE —v PI ti 

: : : tə | = 0. (7.27) 
Py 0 —unP} | \ts 


0 PL —vy P] 


由 于 t 一 共有 12 维 ， 因 此 ， 最 少 通 过 6 对 匹配 点 即 可 实现 矩阵 了 的 线性 
求解 ， 这 种 方法 称 为 直接 线性 变换 (Direct Linear Transform, DLT) . 4 
匹配 点 大 于 6 对 时 ， 也 可 以 使 用 SVD 等 方法 对 超 定 方程 求 最 小 二 乘 解 。 


在 DLT 求 解 中 ， 我 们 直接 将 T 和 矩阵 看 成 了 12 个 未 知 数 ， 忽 略 了 它们 之 
间 的 联系 。 因 为 旋转 和 矩阵 RE SO(3)， 用 DLT 求 出 的 解 不 一 定 满足 该 约 
束 ， 它 是 一 个 一 般 和 矩阵 。 平 移 向 量 比较 好 办 ， 它 属于 向 量 空 间 。 对 于 旗 
FIEBER ， 我 们 必须 针对 DLT 估 计 的 了 左边 3x 3 的 矩阵 块 ， 寻 找 一 个 最 好 
的 旋转 矩阵 对 它 进行 近似 。 这 可 以 由 QR 分 解 完成 5 ， 相 当 于 把 结果 从 
和 矩 阵 空间 重新 投影 到 SE(3) 流 形 上 ， 转 换 成 旋转 和 平移 两 部 分 。 


需要 解释 的 是 ， 我 们 这 里 的 x , 使 用 了 归 一 化 平面 坐标 ， 去 掉 了 内 参 


AEBEK 的 影响 一 一 这 是 因为 内 参 K 在 SLAM 中 通常 假设 为 已 知 。 即 使 内 参 
未 知 ， 也 能 用 PnP 去 估计 K,R,t 三 个 量 。 然 而 由 于 未 知 量 增多 ， 效 果 会 差 


— tb 
— Q 


7.7.2 P3P 


下 面 讲 的 P3P 是 另 一 种 解 PnP 的 方法 。 它 仅 使 用 3 对 匹配 点 ， 对 效 据 要 
求 较 少 ， 因 此 这 里 也 简单 介绍 一 下 (这 部 分 推导 借鉴 了 文献 [49]) 。 

P3P 需 要 利用 给 定 的 3 个 点 的 几何 关系 。 它 的 输入 数据 为 3 对 3D-2D 匹 
配点 。 记 3D 点 为 A,B,C ，2D 点 为 a,b,c ， 其 中 小 写字 母 代 表 的 点 为 对 应 大 
写字 母 代表 的 点 在 相机 成 像 平面 上 的 投影 ， 如 图 7-11 所 示 。 此 外 ，P3P 还 
需要 使 用 一 对 验证 点 ， 以 从 可 能 的 解 中 选 出 正确 的 那 一 个 (类 似 于 对 极 


几何 情形 ) 。 记 验证 点 对 为 D-d ， 相 机 光 心 为 O 。 请 注意 ， 我 们 知道 的 是 
A,B,C 在 世界 坐标 系 中 的 坐标 ， 而 不 是 在 相机 坐标 系 中 的 坐标 。 一 旦 3D 
点 在 相机 坐标 系 下 的 坐标 能 够 算出 ， 我 们 就 得 到 了 3D-3D 的 对 应 点 ， 把 
PnP 问题 转换 为 了 ICP 问 题 。 


首先 ， 显 然 三 角形 之 间 存 在 对 应 天 系 : 
O 


AOab— AOAB, AObc— AOBC, AOac— AOAC. (7.28) 
NS 
k , 


B 
图 7-11 _ P3P 问 题 示意 图 。 


来 考虑 Oab 和 OAB 的 关系 。 利 用 余弦 定理 ， 有 : 
OA? + OB? — 20A- OB -cos (a,b) = AB?. (7.29) 
对 于 其 他 两 个 三 角形 亦 有 类 似 性 质 ， 于 是 有 : 


OA? + OB? — 204A - OB - cos (a,b) = AB? 
OB? + OC? — 20B - OC - cos (b,c) = BC? (7.30) 
OA? + OC? — 20A - OC - cos (a,c) = AC?. 


对 以 上 三 式 全 体 除 以 0C : , ##Bidx =OA /OC,y =OB/OC ， 得 : 


x? +y? — 2ry cos (a,b) = AB? /OC? 
y? + 1? — 2y cos (b,c) = BC? /OC? (7.31) 
x? + 1? — 2x cos (a,c) = AC*/OC?. 


记 v =AB ? /OC ? ,uv =BC ? /OC ? ,wv =AC?/OC?, B: 


x£? +y? — 2xy cos (a,b) — v = 0 
y? + 1? — 2y cos (b,c) — uv = 0 (7.32) 


x? + 1? — 2z cos (a,c) — wv = 0. 
我 们 可 以 把 第 一 个 式 子 中 的 v 放 到 等 式 一 边 ， 并 代入 其 后 两 式 ， 得 : 


(1 — u) y? — ux? — cos (b,c) y + 2uxy cos (a,b) +1=0 was 
(1 — w) x? — wy? — cos (a,c) x + 2waxy cos (a,b) + 1 = 0. 


注意 这 些 方程 中 的 已 知 量 和 未 知 量 。 由 于 我 们 知道 2D 点 的 图 像 位 
置 ，3 个 余弦 角 cos (a,b) ,cos 《b,c》, cos (ac) 是 已 知 的 。 同 时 , u 
=BC ? /AB ? ,w =AC ? /AB ? 可 以 通过 A,B,C 在 世界 坐标 系 下 的 坐标 算出 ， 变 
换 到 相机 坐标 系 下 之 后 ， 这 个 比值 并 不 改变 。 该 式 中 的 xy 是 未 知 的 ， 随 
着 相机 移动 会 发 生变 化 。 因 此 ， 该 方程 组 是 关于 xy 的 一 个 二 元 二 次 方程 

(多 项 式 方程 ) 。 解 析 地 求解 该 方程 组 是 一 个 复杂 的 过 程 ， 需 要 用 吴 消 
元 法 。 这 里 不 展开 对 该 方程 解法 的 介绍 ， 感 兴趣 的 读者 请 参阅 文献 [45]。 
类 似 于 分 解 E 的 情况 ， 访 方程 最 多 可 能 得 到 4 个 解 ， 但 我 们 可 以 用 验证 点 
来 计算 最 可 能 的 解 ， 得 到 A,B,C 在 相机 坐标 系 下 的 3D 坐 标 。 然 后 ， 根 据 
3D-3D 的 点 对 ， 计 算 相 机 的 运动 R,t 。 这 部 分 将 在 7.9 节 介绍 。 


从 P3P 的 原理 可 以 看 出 ， 为 了 求解 PnP， 我 们 利用 了 三 角形 相似 性 
质 ， 求 解 投影 点 a,b,c 在 相机 坐标 系 下 的 3D 坐 标 ， 最 后 把 问题 转换 成 一 个 
3D 到 3D 的 位 次 估计 问题 。 在 后 文 将 看 到 ， 带 有 匹配 信息 的 3D-3D 位 姿 求 
解 非常 容易 ， 所 以 这 种 思路 是 非常 有 效 的 。 其 他 的 一 些 方 法 ， 例 如 
EPnP， 亦 采用 了 这 种 思路 。 然 而 ，P3P 也 存在 着 一 些 问题 : 


1.P3P 只 利用 3 个 点 的 信息 。 当 给 定 的 配对 点 多 于 3 组 时 ， 难 以 利用 更 
多 的 信息 。 


2.90R3D RRR, REFERA, WAAR 


所 以 后 续 人 们 还 提出 了 许多 别 的 方法 ， 如 EPnP、UPnP 等 。 它 们 利用 
更 多 的 信息 ， 而 且 用 和 迭代 的 方式 对 相机 位 姿 进 行 优 化 ， 以 尽 可 能 地 消除 
噪声 的 影响 。 不 过 ， 相 对 于 P3P 来 说 ， 原 理会 更 加 复杂 一 些 ， 所 以 我 们 建 
议 读者 阅读 原始 的 论文 ， 或 通过 实践 来 理解 PnP 过 程 。 在 SLAM 当 中 ， 通 
常 的 做 法 是 先 使 用 P3P/EPnP 等 方法 估计 相机 位 姿 ， 然 后 构建 最 小 二 乘 优 
化 问题 对 估计 值 进行 调整 (Bundle Adjustment) 。 接 下 来 我 们 从 非 线 性 优 
化 角度 来 看 一 下 PnP 问题 。 


7.7.3 Bundle Adjustment 


除了 使 用 线性 方法 之 外 ， 我 们 还 可 以 把 PnP 问题 构建 成 一 个 定义 于 李 
代数 上 的 非 线 性 最 小 二 乘 问 题 。 这 将 用 到 本 书 第 4 讲 和 第 5 讲 的 知识 。 前 
面 说 的 线性 方法 ， 往 往 是 先 求 相 机 位 姿 ， 再 求 空间 点 位 置 ， 而 非 线性 优 
化 则 是 把 它们 都 看 成 优化 变量 ， 放 在 一 起 优化 。 这 是 一 种 非常 通用 的 求 
解 方 式 ， 我 们 可 以 用 它 对 PnP 或 1CP 给 出 的 结果 进行 优化 。 在 PnP 中 ， 这 个 
Bundle Adjustment 问 题 ， 是 一 个 最 小 化 重 投影 误差 ( Reprojection error) 
的 问题 。 我 们 将 在 本 节 给 出 此 问题 在 两 个 视图 下 的 基本 形式 ， 然 后 在 第 
10 讲 讨论 较 大 规模 的 BA 问题 。 

考虑 n 个 三 维 空间 点 P 及 其 投影 p ， 我 们 希望 计算 相机 的 位 姿 R,t ， 它 
的 李 代数 表示 为 5E。 假 设 某 空 间 点 坐标 为 P =[X,,Y ,2Z,] ， 其 投影 的 像素 
坐标 为 w =[u, ,v, ]。 根 据 第 5 讲 的 内 容 ， 像 素 位 置 与 空间 点 位 置 的 关系 如 


Si | vi = K exp (E^) 


x; 

Y; 
(7.34) 

Zi 

1 


除了 用 上 表示 相机 姿态 之 外 ， 别 的 都 和 前 面 的 定义 保持 一 致 。 写 成 答 
阵 形式 就 是 : 


siu; = K exp (£^) 万 . 


请 读者 脑 补 中 间 隐 含 着 的 齐 次 坐标 到 非 齐 次 的 转换 ， 否 则 按 和 矩阵 的 
乘法 来 说 ， 维 度 是 不 对 的 外 。 现 在 ， 由 于 相机 位 姿 未知 及 观测 点 的 品 
声 ， 该 等 式 存在 一 个 误差 。 因 此 ， 我 们 把 误差 求 和 和， 构建 最 小 二 乘 间 
题 ， 然 后 寻找 最 好 的 相机 位 姿 ， 使 它 最 小 化 : 


1 nm 
* 一 in 一 7.35 
E arg min 5 >, a ( ) 


1 2 
u; — —K exp (£^) P;|| . 
2 


该 问题 的 误差 项 ， 是 将 像素 坐标 (观测 到 的 投影 位 置 ) 与 3D 点 按照 
当前 估计 的 位 姿 进 行 投影 得 到 的 位 置 相 比较 得 到 的 误差 ， 所 以 称 为 重 投 
影 误差 。 使 用 齐 次 坐标 时 ， 这 个 误差 有 3 维 。 不 过 ， 由 于 v 最 后 一 维 为 
1， 该 维度 的 误差 一 直 为 零 ， 因 而 我 们 更 多 时 候 使 用 非 齐 次 坐标 ， 于 是 误 
差 就 只 有 2 维 了 。 如 图 7-12 所 示 ， 我 们 通过 特征 匹配 知道 了 p , 和 p , =a 
一 个 空间 点 P 的 投影 ， 但 是 不 知道 相机 的 位 姿 。 在 初始 值 中 ，P 的 投影 7 
, 与 实际 的 p , 之 间 有 一 定 的 距离 。 于 是 我 们 调整 相机 的 位 姿 ， 使 得 这 个 距 
离 变 小 。 不 过 ， 由 于 这 个 调整 需要 考虑 很 多 个 点 ， 所 以 最 后 每 个 点 的 误 
差 通常 都 不 会 精确 为 零 。 

最 小 二 乘 优化 问题 已 经 在 第 6 讲 介 绍 过 了 。 使 用 李 代 数 ， 可 以 构建 无 
约束 的 优化 问题 ， 很 方便 地 通过 高 斯 牛顿 法 、 列 文 伯 格 一 马 硅 尔 特 方法 
等 优化 算法 进行 求解 。 不 过 ， 在 使 用 高 斯 牛顿 法 和 列 文 伯 格 一 马 夸 尔 特 
方法 之 前 ， 我 们 需要 知道 每 个 误差 项 关于 优化 变量 的 导数 ， 也 就 是 线性 
tt: 


e(x + Ax) = e(x) + JAz. (7.36) 


图 7-12” 重 投影 误差 示意 图 。 


这 里 的 JJ 的 形式 是 值得 讨论 的 ， 甚 至 可 以 说 是 关键 所 在 。 我 们 固然 可 
以 使 用 数值 导数 ， 但 如 果 能 够 推导 出 解析 形式 ， 则 我 们 会 优先 考虑 解析 
导数 。 现 在 ， 当 e 为 像素 坐标 误差 (2 维 ) ，x 为 相机 位 姿 (6 维 ) 时 ，J 
将 是 一 个 2x 6 的 矩阵 。 我 们 来 推导 JJ 的 形式 。 

回忆 李 代 数 的 内 容 ， 我 们 介绍 了 如 何 使 用 扰动 模型 来 求 李 代数 的 导 
数 。 首 先 ， 记 变换 到 相机 坐标 系 下 的 空间 点 坐标 为 P ， 并 且 将 其 前 3 维 取 
出 来 : 


P=(exp(e YP). SIR YZ| (7.37) 


那么 ， 相 机 投影 模型 相对 于 P 为 


su = KP’. (7.38) 
展开 : 
su fe 0 cz X’ 
sw |=| 0 fy & Y |> (7.39) 
S 0 0 1 WA 


利用 第 3 行 消去 s (实际 上 就 是 P' 的 距离 ) ， 得 : 


/ 


w= fi bee v= fys tty. (7.40) 
这 与 之 前 讲 的 相机 模型 是 一 致 的 。 当 我 们 求 误差 时 ， 可 以 把 这 里 的 
uv 与 实际 的 测量 值 比较 ， 求 差 。 在 定义 了 中 间 变 量 后 ， 我 们 对 &^ ARH 


动量 565 ， 然 后 考虑 e 的 变化 关于 扰动 量 的 导数 。 利 用 链 式 法 则 ， 可 以 列 写 
如 下 : 


ðe Ve e(6EDE) Oe OP" 
O56E€ 6€30 SE ~ OP’ asé 


这 里 的 @ 指 李 代数 上 的 左 乘 扰动 。 第 一 项 是 误差 天 于 投影 点 的 导 
数 ， 在 式 (7.40) 已 经 列 出 了 变量 之 间 的 关系 ， 易 得 : 


fe} (a) fe} fa fax” 
ðe ahs ax? BY? DZ sy | 2 0 —GR (7.42) 
OP’ Ov Ov Ov 0 Sy fay ` f 
Z! 7 


Ox’ OY’ OZ! 


(7.41) 


而 第 二 项 为 变换 后 的 点 关于 李 代 数 的 导数 ， 根 据 4.3.5 节 中 的 推导 ， 


1H 。 
F. 


ô (TP) = (TP)° 三 | es | (7.43) 


而 在 P 的 定义 中 ， 我 们 取出 了 前 3 维 ， 于 是 得 : 


OP! _ 
aoe 7 | 


将 这 两 项 相 乘 ， 就 得 到 了 2x 6697n LOE R: 


I,-P”). (7.44) 


z BP, 2X'Y' aX? sY” 
ðe & 0 ies bx fs + fax -ft 
ye , /2 ne i (7.45) 
O6€ 0 & -Y = fyY fyX 了 fuX 
dg ZZ’2 y Z!2 Zl2 i 


这 个 雅 可 比 和 矩阵 描述 了 重 投影 误差 天 于 相机 位 资 李 代 数 的 一 阶 变 化 
关系 。 我 们 保留 了 前 面 的 负 号 ， 这 是 因为 误差 是 由 观测 值 减 预测 值 定义 
的 。 它 当然 也 可 反 过 来 ， 定 义 成 “预测 值 减 观测 值 * 的 形式 。 在 那 种 情况 
下 ， 只 要 去 掉 前 面 的 负 号 即 可 。 此 外 ， 如 果 se(3) 的 定义 方式 是 旋转 在 
BU, 平移 在 后 ， 只 要 把 这 个 和 矩阵 的 前 3 列 与 后 3 列 对 调 即 可 。 


另 一 万 面 ， 除 了 优化 位 姿 ， 我 们 还 希望 优化 特征 点 的 空间 位 置 。 
此 ， 需 要 讨论 e 关于 空间 点 P 的 导数 。 所 盏 这 个 导数 矩阵 相对 来 说 容易 一 
些 。 仍 利用 链 式 法 则 ， 有 : 


Oe De OP’ 


ðP OP! dP’ re) 
第 一 项 在 前 面 已 推导 ， 关 于 第 二 项 ， 按 照 定义 
P’ = exp(é*)P = RP +t, 
我 们 发 现 P 对 P 求 导 后 将 只 剩 下 R。 于 是 : 
fz _ fX” 
ae == : i R. (7.47) 


于 是 ， 我 们 推导 出 了 观测 相机 方程 关于 相机 位 资 与 特征 点 的 两 个 导 
数 矩 阵 。 它 们 十 分 重要 ， 能 够 在 优化 过 程 中 提供 重要 的 梯度 方向 ， 指 导 
HEITAR 


7.8 ”实践 : 求解 PnP 
7.8.1 ”使 用 EPnP 求 解 位 姿 


下 面 ， 我 们 通过 实验 理解 一 下 PnP 的 过 程 。 首先 ， 我 们 用 OpenCV 提 
供 的 EPnP 求 解 Pnp 问 题 ， 然 后 通过 g2o 对 结果 进行 优化 。 由 于 PnP 需要 使 
用 3D 点 ， 为 了 避免 初始 化 带 来 的 麻烦 ， 我 们 使 用 了 RGB-D 相 机 中 的 深度 
(1_depth png) 作为 特征 点 的 3D 位 置 。 首 先 来 看 OpenCvV 提 供 的 PnP 函 
数 : 


办 slambook/ch7/pose_estimation_3d2d.cpp (片段 ) 


1 | int main( int argc, char** argv ) 
ae | 省 
3 | me 
4 // 建立 3D 点 
5 Mat dl = imread( argv[3] CV_LOAD_IMAGE_UNCHANGED ) ; // 深度 图 为 16 位 无 
符号 数 ， 单 通道 图 像 
6 Mat K = ( Mat_<double> ( 3,3 ) << 520.9，0，325.1，0，521.0，249.7，0，0， 
7 vector<Point3f> pts_3d; 
8 vector<Point2f> pts_2d; 
9 for ( DMatch m:matches ) 
10 { 
1 ushort d = di.ptr<unsigned short> ( int(keypoints_1[m.queryIdx].pt.y) )[ 
int (keypoints_1[m.queryIdx].pt.x) ]; 
2 if (d==0) // bad depth 
13 continue; 
14 float dd = d/1000.0; 
15 Point2d p1 = pixel2cam( keypoints_1[m.queryIdx].pt, K ); 
16 pts_3d.push_back( Point3f(p1.x*dd, p1.y*dd, dd) ); 
17 pts_2d.push_back ( keypoints_2[m.trainIdx].pt ); 
18 } 
19 
20 cout<<"3d-2d pairs: "<<pts_3d.size()<<endl; 
21 
22 Mat r, t; 
23 // 调用 OpenCV 的 PnP 求解 ， 可 选择 EPNP. DLS FAK 
24 solvePnP( pts_3d, pts_2d, K, Mat(), r, t, false, cv::SOLVEPNP_EPNP ); 
25 Mat R; 
26 cv::Rodrigues(r, R); // r 为 旋转 向 量 形式 ， 用 Rodrigues 公式 转换 为 矩阵 
27 
28 cout<<"R="<<end1<<R<<end1; 
29 cout<<"t="<<endl<<t<<end1; 
30 |} 


在 例 程 中 ， 得 到 配对 特征 点 后 ， 我 们 在 第 一 个 图 的 深度 图 中 寻找 它 


们 的 深度 ， 并 求 出 空 


间 人 位置。 以 此 空间 位 置 为 3D 点 ， 再 以 第 二 个 图 像 的 


像素 位 置 为 2D 点 ， 调 用 EPnP 求 解 PnP 问 题 。 程 序 输出 如 下 : 


1 |% build/pose_estimation_3d2d 1.png 2.png di.png d2.png 
2 |-- Max dist : 95.000000 
3 |-- Min dist : 4.000000 
4 | 一 共 找到 了 79 组 匹配 点 
5 |3d-2d pairs: 78 
6 | R= 

7 | [0.9977970937403702, -0.05195299069131867, 0.04125344205637558; 
s | 0.05073872610592159, 0.9982626103770279, 0.0299556738597 2873; 

9 | -0.04273805559942161, -0.02779653722084675, 0.9986995599889442] 
10 | t= 

u | [-0.6455324432075111; 

12 | -0.05776758294184359 ; 

13 | 0.2844565219506077] 


读者 可 以 对 比 先前 2D-2D 情 况 下 求解 的 R,t 看 看 有 什么 不 同 。 可 以 看 
到 ， 在 有 3D 信 息 时 ， 估 计 的 R 几乎 是 相同 的 ， 而 t 相差 得 较 多 。 这 是 由 于 
引入 了 新 的 深度 信息 所 致 。 不 过 ， 由 于 Kinect 采 集 的 深度 图 本 身 会 有 一 些 
误差 ， 所 以 这 里 的 3D 扣 也 不 是 准确 的 。 我 们 会 希望 把 位 姿 E 和 所 有 三 维 


FFE RAP 同时 优化 。 


7.8.2 ”使 用 BA 优化 


下 面 演 示 如 何 进 行 BundleAdjustment。 我 们 将 使 用 前 一 步 的 估计 值 作 
为 初始 值 。 优 化 可 以 使 用 前 面 讲 的 Ceres 或 g2o 库 实现 ， 这 里 采用 g2o 作 为 


例子 。 


g2o 的 基本 知识 在 第 6 讲 中 已 经 介绍 过 了 。 在 使 用 g2o 之 前 ， 我 们 要 把 


问题 建 模 成 一 个 最 小 二 乘 的 图 优化 问题 ， 如 图 7-13 所 示 。 


Q P,  g2o::VertexSBAPointXYZ 


g2o0::EdgeProjectXYZ2UV 


a= MEP | 


See e a se 人 1) 
R,t 
g2o0::VertexSE3Expmap 


7-13 ”PnP 的 Bundle Adjustment 的 图 优化 表示 。 


在 这 个 图 优化 中 ， 节 点 和 边 的 选择 如 下 : 


1. 节 点 : 第 二 个 相机 的 位 姿 节 点 5E se(3)， 所 有 特征 点 的 空间 位 置 
PER’. 


2.30: 每 个 3D 点 在 第 二 个 相机 中 的 投影 ， 以 观测 方程 来 描述 : 


由 于 第 一 个 相机 位 姿 固定 为 零 ， 我 们 没有 把 它 写 到 优化 变量 里 ， 但 
在 习题 中 ， 希 望 你 能 够 把 第 一 个 相机 的 位 姿 与 观测 也 考虑 进来 。 现 在 我 
们 根据 一 组 3D 点 和 第 二 个 图 像 中 的 2D 投 影 ， 估 计 第 二 个 相机 的 位 姿 。 所 
以 我 们 把 第 一 个 相机 画 成 虚线 ， 表 明 不 希望 考虑 它 。 

g2o 提 供 了 许多 关于 BA 的 节点 和 边 ， 我 们 不 必 自 己 从 头 实现 所 有 的 
计算 。 在 g20/types/sba/types_six_dof_expmap.h 中 则 提供 了 李 代 数 表达 的 节 
点 和 边 。 请 读者 打开 这 个 文件 ， 找 到 VertexSE3Expmap 〈 李 代数 位 姿 ) 、 
VertexSBAPointXYZ (lal Rafi) 和 EdgeProjectXYZ2UV (投影 方程 
边 ) 这 三 个 类 。 我 们 来 简单 看 一 下 它们 的 类 定义 ， 例 如 
VertexSE3Expmap: 


class G20_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{ 


N 


public: 
3 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 


5 VertexSE3Expmap () ; 

6 

7 bool read(std::istream& is); 

8 

9 bool write(std::ostreamk os) const; 


10 

11 virtual void setToOriginImpl() { 

12 _estimate = SE3Quat(); 

13 J 

14 

15 virtual void oplusImpl(const double* update_) { 
16 Eigen: :Map<const Vector6d> update (update_) ; 

17 setEstimate( SE3Quat: :exp(update)*estimate()) ; 
18 } 

i9 |}; 


请 注意 它 的 模板 参数 。 第 一 个 参数 6 表示 它 内 部 存储 的 优化 变量 维 
度 ， 可 以 看 到 这 是 一 个 6 维 的 李 代 数 。 第 二 个 参数 是 优化 变量 的 类 型 ， 这 
里 使 用 了 g2o 定 义 的 相机 位 姿 : SE3Quat。 这 个 类 内 部 使 用 了 四 元 数 加 位 
移 向 量 来 存储 位 资 ， 但 同时 也 支持 李 代数 上 的 运算 ， 例 如 对 数 映射 (log 
函数 ) 和 李 代 数 上 增 量 《update 函数 ) 等 操作 。 我 们 可 以 对 照 它 的 实现 代 
码 ， 看 看 g2o 对 李 代 效 是 如 何 操作 的 : 


1 | class G20_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3D> 

2 |{ 

3 | eevee 

4 |}; 

5 

6 |class G20_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2D, 

VertexSBAPointXxYZ, VertexSE3Expmap> 

7 |{ 

8 | sw ata: ae 

9 void computeError() { 

10 const VertexSE3Expmap* vi = static_cast<const VertexSE3Expmap*>(_vertices 
[1]); 

11 const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>( 
_vertices[0]); 

12 const CameraParameters * cam 

13 = static_cast<const CameraParameters *>(parameter(0)); 

14 Vector2D obs(_measurement) ; 

15 _error = obs-cam->cam_map(vi->estimate() .map(v2->estimate())); 

16 } 

iz | Fs 


文 里 就 不 把 整个 类 定义 都 搬 过 来 了 。 从 模板 参数 可 以 看 到 ， 空 间 点 
位 置 类 的 维度 为 3， 类 型 是 Eigen 的 Vector3D 。 另 一 方面 ， 边 
EdgeProjectXYZ2UV 连 接 了 前 面 说 的 两 个 顶点 ， 它 的 观测 值 为 2 维 ， 5 
Vector2D 表 示 ， 实 际 上 就 是 空间 点 的 像素 坐标 。 它 的 误差 计算 函数 表达 
投影 方程 的 -o 法 ， 也 就 是 我 们 前 面 提 到 的 z-h (&,P ) 的 方式 。 


现在 ， 进 一 步 观察 EdgeProjectXYZ2UV 的 linearizeOplus 孙 数 的 实现 。 
这 里 用 到 了 m 前 面 推导 的 雅 可 比 和 矩阵 : 


void EdgeProjectXYZ2UV::linearizeOplus() { 
VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]); 
SE3Quat T(vj->estimate()); 
VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]); 
Vector3D xyz = vi->estimate(); 


Vector3D xyz_trans = T.map(xyz) ; 


double x = xyz_trans[0]; 


double y = xyz_trans[1]; 


double z = xyz_trans[2]; 


double z_2 = z*z; 


const CameraParameters * cam = static_cast<const CameraParameters *>(parameter 


(0)); 


Matrix<double,2,3,Eigen::ColMajor> tmp; 
tmp(0,0) = cam->focal_length; 


tmp(0,1) = 0; 

tmp(0,2) = -x/z*cam->focal_length; 

tmp(1,0) = 0; 

tmp(1,1) = cam->focal_length; 

tmp(1,2) = -y/z*cam->focal_length; 

_jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix() ; 


_jacobian0plusxj (0,0) x*y/z_2 *cam->focal_length; 
_jacobianOplusxj(0,1) = -(1+(x*x/z_2)) *cam->focal_length; 
_jacobian0plusxj(0,2) = y/z *cam->focal_length; 
_jacobianOplusXj (0,3) = -1./z *cam->focal_length; 
_jacobianOplusxj (0,4) = 0; 

_jacobianOplusxj (0,5) = x/z_2 *cam->focal_length; 


_jacobianOplusxj(1,0) = (1t+y*y/z_2) *cam->focal_length; 
_jacobianOplusXj(1,1) = -x*y/z_2 *cam->focal_length; 
_jacobianOplusXj(1,2) = -x/z *cam->focal_length; 
_jacobian0plusXj(1,3) = 0; 

_jacobian0plusxj (1,4) = -1./z *cam->focal_length; 
_jacobianOplusXj(1,5) = y/z_2 *cam->focal_length; 


仔细 研究 此 段 代 码 ， 我 们 会 发 现 它 与 式 (7.45) 和 (7.47) 是 一 致 的 。 成 员 
变量 ″ jacobianOplusXi" 是 误差 到 空间 点 的 导数 ，“ jacobianOplusXj” 是 误 
差 到 相机 位 姿 的 导数 ， 以 李 代 数 的 左 乘 扰动 表达 。 稍 有 差别 的 是 ，g20 的 
相机 里 用 f 统一 描述 f. ,f, ， 并 且 李 代数 定义 顺序 不 同 〈g2o 是 旋转 在 前 ， 平 
BHA; 我 们 是 平移 在 前 ， 旋 转 在 后 ) ， 所 以 矩阵 前 3 列 和 后 3 列 与 我 们 
的 定义 是 相反 的 ， 此 外 都 一 致 。 

值得 一 提 的 是 ， 我 们 亦 可 自己 实现 相机 位 姿 节点 ， 并 使 用 
Sophus::SE3 来 表达 位 姿 ， 提 供 类 似 的 求 导 过 程 。 然 而 ， 既 然 g20 已 经 提供 
了 这 样 的 类 ， 在 没有 额外 要 求 的 情况 下 ， 目 己 重 新 实现 就 没有 必要 了 。 
现在 ， 我 们 在 上 一 个 PnP 例 程 的 基础 上 上， 加 上 g2o 提 供 的 Bundle 
Adjustmento 


四 slambook/ch7/pose_estimation_3d2d.cpp (片段 ) 


void bundleAdjustment ( 
const vector< Point3f > points_3d, 
const vector< Point2f > points_2d, 
const Mat& K, 
Mat& R, Mat& t ) 


// 初始 化 g20 

typedef g20::BlockSolver< g20::BlockSolverTraits<6,3> > Block; // pose 维度 为 
6, landmark 维度 为 3 

Block::LinearSolverType* linearSolver = new g20::LinearSolverCSparse<Block:: 
PoseMatrixType>() ; 

Block* solver_ptr = new Block( linearSolver ); 
g20::OptimizationAlgorithmLevenberg* solver = new g2o:: 
OptimizationAlgorithmLevenberg( solver_ptr ); 

g20::SparseOptimizer optimizer; 


optimizer.setAlgorithm( solver ); 


// vertex 
g20::VertexSE3Expmap* pose = new g20::VertexSE3Expmap(); // camera pose 
Eigen: :Matrix3d R_mat; 
R_mat << 
R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), 
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), 
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2); 
pose->setId(0) ; 
pose->setEstimate( g2o::SE3Quat ( 
R_mat, 
Eigen: :Vector3d( t.at<double>(0,0), t.at<double>(1,0), t.at<double>(2,0)) 
J. J 


optimizer.addVertex( pose ); 


int index = 1; 


for ( const Point3f p:points_3d ) // landmarks 


{ 
g2o0::VertexSBAPointXYZ* point = new g20::VertexSBAPointXYZ() ; 
point->setId( index++ ); 
point->setEstimate( Eigen::Vector3d(p.x, p.y, p-z) ); 
point->setMarginalized( true ); 
optimizer.addVertex( point ); 

} 


// parameter: camera intrinsics 

g2o0::CameraParameters* camera = new g20::CameraParameters( 
K.at<double>(0,0), Eigen: :Vector2d(K.at<double>(0,2), K.at<double>(1,2)), 0 
) ; 

camera->setId(0) ; 


optimizer.addParameter( camera ) ; 


// edges 

index = 1; 

for ( const Point2f p:points_2d ) 

£ 
g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV(); 
edge->setId( index ); 
edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer .vertex 
(index)) ); 
edge->setVertex( 1, pose ); 
edge->setMeasurement( Eigen::Vector2d( p.x, p.y ) ); 
edge->setParameterId(0,0) ; 
edge->setInformation( Eigen: :Matrix2d::Identity() ); 
optimizer .addEdge (edge) ; 


index++; 


chrono::steady_clock::time_point tl = chrono::steady_clock::now(); 
optimizer.setVerbose( true ); 
optimizer.initializeOptimization() ; 


optimizer .optimize (100) ; 


1 


chrono: :steady_clock::time_point t2 chrono: :steady_clock::now(); 
chrono: :duration<double> time_used = chrono: :duration_cast<chrono: :duration< 
double>>(t2-t1); 


cout<<"optimization costs time: "<<time_used.count()<<" seconds."<<endl; 


cout<<endl<<"after optimization:"<<endl; 


cout<<"T="<<endl<<Eigen: :Isometry3d( pose->estimate() ).matrix()<<endl; 


程序 大 体 上 和 第 6 讲 的 g2o 类 似 。 我 们 首先 声明 了 g2o 图 优化 ， 并 配置 
优化 求解 器 和 梯度 下 降 方 法 。 然 后 根据 估计 到 的 特征 点 ， 将 位 资 和 空间 
点 放 到 图 中 。 最 后 调用 优化 疯 数 进行 求解 。 读 者 可 以 看 到 优化 的 结果 如 
Te 


1 | calling bundle adjustment 

2 |iteration= 0 chi2= 1.083180 time= 0.000107183 cumTime= 0.000107183 edges= 76 schur 
= 1 lambda= 78.907222 levenbergIter= 1 

3 | iteration= 1 chi2= 0.000798 time= 5.8615e-05 cumTime= 0.000165798 edges= 76 schur 
= 1 lambda= 26.302407 levenbergIter= 1 

4 |iteration= 2 chi2= 0.000000 time= 3.0203e-05 cumTime= 0.000196001 edges= 76 schur 
= 1 lambda= 17.534938 levenbergIter= 1 

本 中 间 过 程 略 

6 | iteration= 11 chi2= 0.000000 time= 2.8394e-05 cumTime= 0.000525203 edges= 76 schur 
= 1 lambda= 11209.703029 levenbergIter= 1 


7 | optimization costs time: 0.00132938 seconds. 


9 |after optimization: 

10 | T= 

u |0.997776 -0.0519476 0.0417755 -0.649778 

2 |0.050735 0.998274 0.0295806 -0.0545231 

3 | -0.0432401 -0.0273953 0.998689 0.295564 
14 |0 0 0 1 
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变 ， 于 是 停止 优化 。 我 们 输出 了 最 后 得 到 的 位 次 变换 和 矩阵 7 ， 对 比 之 前 直 
接 做 PnP 的 结果 ， 大 约 在 小 数 点 后 第 3 位 发 生 了 一 些 变 化 。 这 主要 是 由 于 
我 们 同时 优化 了 特征 点 和 相机 位 资 导致 的 。 


Bundle Adjustment 是 一 种 通用 的 做 法 。 它 可 以 不 限于 两 幅 图 像 。 我 们 
完全 可 以 放 入 多 幅 图 像 匹 配 到 的 位 姿 和 空间 点 进行 迭代 优化 ， 甚 至 可 以 
把 整个 SLAM 过 程 放 进来 。 那 种 做 法 规模 较 大 ， 主 要 在 后 端 使 用 ， 我 们 会 
ee Sale RIIN, 我 们 通 单 考虑 局 部 相机 位 资 和 特 

正点 的 小 型 Bundle Adjustment 问题， 希望 对 它 进 行 实时 求解 和 优化 。 


7.9 3D-3D. ICP 


最 后 ， 我 们 来 介绍 3D-3D 的 位 次 估计 问题 。 假 设 我 们 有 一 组 配对 好 的 
3D 点 〈 比 如 我 们 对 两 幅 RGB-D 图 像 进行 了 匹配 ) : 


P = {pi1,.… ,pn}, P’ = {p}; Php 
现在 ， 想 要 找 一 个 欧 氏 变换 R,t ， 使 得 : 
Vi, Pi = Rp’ +t. 


这 个 问题 可 以 用 迭代 最 近 点 (Iterative Closest Point, ICP) 求解 。 读 
者 应 该 注意 到 了 ，3D-3D 位 姿 估 计 间 题 中 并 没有 出 现 相机 模型 ， 也 就 是 
说 ， 仅 考虑 两 组 3D 点 之 间 的 变换 时 ， 和 相机 并 没有 关系 。 因 此 ， 在 激光 
SLAM 中 也 会 碰 到 ICP， 不 过 由 于 激光 数据 特征 不 够 丰富 ， 我 们 无 从 知道 
两 个 点 集 之 间 的 匹配 关系 ， 只 能 认为 距离 最 近 的 两 个 点 为 同一 个 ， 所 以 
这 个 方法 称 为 迭代 最 近 点 。 而 在 视觉 中 ， 特 征 点 为 我 们 提供 了 较 好 的 匹 
配 关 系 ， 所 以 整个 问题 就 变 得 更 简单 了 。 在 RGB-D SLAM 中 ， 可 以 用 这 
种 方式 估计 相机 位 姿 。 下 文 我 们 用 ICP 指 代 匹 配 好 的 两 组 点 间 的 运动 估计 
问题 。 

和 PnP 类 似 ，ICP 的 求解 也 分 为 两 种 方式 : 利用 线性 代数 的 求解 ( 主 
要 是 SVD) ， 以 及 利用 非 线 性 优化 方式 的 求解 (类似 于 Bundle 
Adjustment) 。 下 面 分 别 进行 介绍 。 


7.9.1 ”SVD 方法 


首先 来 看 以 SVD 为 代表 的 代数 方法 。 根 据 前 面 描述 的 ICP 问 题 ， 我 们 
先 定义 第 i TRAM: 


ei = pi — (Rp, +t). (7.48) 


然后 ， 构 建 最 小 二 乘 问题 ， 求 使 误差 平方 和 达到 极 小 的 Rit : 
min J = 2. (p: — (Rp;' + t))||2. (7.49) 


下 面 来 推导 它 的 求解 方法 。 首 先 ， 定 义 两 组 点 的 质心 : 


p= Dp), p'=—> (7%). (7.50) 


i=1 =l 


WTS, MERA FA Aa, TRÆR AA RAE: 


(7 


2 È lp: 一 Rp -t -p+ Rp’ +p- Rp 
= 3 > ||P: -p - Rp - p')) + (p - Rp! 一 世上 
> (lip: - p- Rp -Pp + Iip — Rp’ — tl?’ + 
2(pi — p — R (p;' — p'))” (p— Rp’ — t)). 
注意 到 交叉 项 部 分 中 (p, -p-R p, -p )) 在 求 和 之 后 为 零 ， 因 此 优化 目标 
函数 可 以 简化 为 


， _ TI / 八 ||12 / 2 
gip I = 5 2 lip p—R(p; —p)|| + |p- Rp — t|". (7.51) 


仔细 观察 左右 两 项 ， 我 们 发 现 左 边 只 和 旋转 矩阵 R 相关 ， 而 右边 既 有 
R 也 有 t ， 但 只 和 质心 相关 。 只 要 我 们 获得 了 R ， 令 第 二 项 为 零 就 能 得 到 t 
。 于 是 ，ICP 可 以 分 为 以 下 三 个 步骤 求解 : 


1. 计 算 两 组 点 的 质心 位 置 P,pP ， 然 后 计算 每 个 点 的 去 质心 坐标 : 
G=Pi-P, q =p,- p. 


2. 根 据 以 下 优化 问题 计算 旋转 矩阵: 


1 n 
R* = arg min 2 ` lq; — Rall”. (7.52) 
i=1 
3. 根 据 第 2 步 的 R 计算 t : 
t* = p— Rp’. (7.53) 


我 们 看 到 ， 只 要 求 出 了 两 组 点 之 间 的 旋转 ， 平 移 量 是 非常 容易 得 到 
的 。 所 以 我 们 重点 关注 R 的 计算 。 展 开关 于 R 的 误差 项 ， 得 : 


1 m 9 1 nm 
52 liq: -Ral = 5) aia + a7 R"Rq; — 2q; Rg. (7.54) 
2 一 工 1=1 


注意 到 第 一 项 和 R 无 关 ， 第 二 项 由 于 RTR = ， 亦 与 RR 无关 。 因 此 ， 
实际 上 优化 目标 函 数 变 为 


5 —q; Rq; = > -tr (Rq;q; ) = —tr [Ry qiqi r) : (7.55) 


i=1 i 二 1 


接 下 来 ， 我 们 介绍 怎样 通过 SVD 解 出 上 述 问 题 中 最 优 的 R 。 关 于 最 优 
性 的 证 明 较 为 复杂 ， 感 兴趣 的 读者 请 参考 文献 [50,51]。 为 了 解 R ， 先 定义 
FER: 


W = >», qiq”. (7.56) 


W 是 一 个 3x 3694ERE, MIWIATTSVDIAB, íF: 
W = UXV". (7.57) 


BW, LAS BZA ERE, WRATCRM ABW) HI, MU 
和 V AT FABRE. SW 满 秩 时 ，R 为 


R=UV'. (7.58) 


解 得 R 后 ， 按 式 (7.53) 求 解 t 即 可 。 


7.9.2” 非 线性 优化 方法 

求解 ICp 的 另 一 种 方式 是 使 用 非 线性 优化 ， 以 迭代 的 方式 去 找 最 优 
值 。 该 方法 和 我 们 前 面 讲述 的 PnP 非常 相似 。 以 李 代数 表达 位 姿 时 ， 目 标 
函数 可 以 写成 


国生 = 2 (pi — exp (£^) p12. (7.59) 


单个 误差 项 关于 位 资 的 导数 在 前 面 已 推导 ， 使 用 李 代 数 扰动 模型 即 
PJ: 


oe _ 
aE 


于 是 ， 在 非 线 性 优化 中 只 需 不 断 迭 代 ， 融 能 找到 极 小 值 。 而 且 ， 可 
以 证 明 @ ，ICP 问 题 存 在 唯一 解 或 无 穷 多 解 的 情况 。 在 唯一 解 的 情况 下 ， 
只 要 能 找到 极 小 值 解 ， 那 么 这 个 极 小 值 就 是 全 局 最 优 值 一 一 因此 不 会 遇 
到 局 部 极 小 而 非 全 局 最 小 的 情况 。 这 也 意味 着 ICP 求 解 可 以 任意 选 定 初始 
值 。 这 是 已 匹配 点 时 求解 ICP 的 一 大 好 处 。 


需要 说 明 的 是 ， 我 们 这 里 讲 的 ICP 是 指 已 由 图 像 特 征 给 定 了 匹配 的 情 
况 下 进行 位 姿 估 计 的 问题 。 在 匹配 已 知 的 情况 下 ， 这 个 最 小 二 乘 问题 实 
际 上 具有 解析 解 包 35 ， 所 以 并 没有 必要 进行 迭代 优化 。ICP 的 研究 者 们 
往往 更 加 关心 匹配 未 知 的 情况 。 不 过 ， 在 RGB-D SLAM 中 ， 由 于 一 个 像 
素 的 深度 数据 可 能 测量 不 到 ， 所 以 我 们 可 以 混合 着 使 用 PnP 和 ICP 优 化 : 
对 于 深度 已 知 的 特征 点 ， 建 模 它们 的 3D-3D 误 差 ， 对 于 深度 未 知 的 特征 
点 ， 则 建 模 3D-2D 的 重 投影 误差 。 于 是 ， 可 以 将 所 有 的 误差 放 在 同一 个 问 
题 中 考虑 ， 使 得 求解 更 加 方便 。 


7.10 XE: 求解 ICP 


7.10.1 SVD 方 法 


—(exp (£^) p;’)®. (7.60) 


下 面 演示 一 下 如 何 使 用 SVD 及 非 线 性 优化 来 求解 ICP。 本 节 我 们 使 用 
两 幅 RGB-D 图 像 ， 通 过 特征 匹配 获取 两 组 3D 点 ， 最 后 用 ICP 计 算 它 们 的 
位 姿 变 换 。 由 于 OpenCV 目 前 还 没有 计算 两 组 带 匹配 点 的 ICP 的 方法 ， 而 
且 它 的 原理 也 并 不 复杂 ， 所 以 我 们 自己 来 实现 一 个 ICP。 


四 slambook/ch7/pose_estimation_3d3d.cpp (片段 ) 


void pose_estimation_3d3d( 


const vector<Point3f>& pts1, 
const vector<Point3f>& pts2, 
Mat& R, Mat& t 


Point3f pl, p2; // center of mass 
int N = ptsi.size(); 


for ( int i=0; i<N; i++ ) 


{ 

pl += ptsi[il; 

p2 += pts2[il; 
} 
pi /= N; p2 /= N; 
vector<Point3f> qi(N), q2(N); // remove the center 
for ( int i=0; i<N; i++ ) 
{ 

gi [i] = ptsi [i] = pi; 

q2[i] = pts2[i] - p2; 
J 


// compute q1*q2T 

Eigen: :Matrix3d W = Eigen: :Matrix3d::Zero() ; 

for ( int i=0; i<N; i++ ) 

{ 
W += Eigen: :Vector3d( qili].x, qi[i].y, qili].z ) * Eigen::Vector3d( q2[i]. 
x, q2[i].y, q2[i].z ).transpose(); 

} 


cout<<"W="<<W<<end1; 


// SVD on W 
Eigen: : JacobiSVD<Eigen: :Matrix3d> svd(W, Eigen: :ComputeFullU|Eigen:: 
ComputeFull1V) ; 


32 Eigen: :Matrix3d U = svd.matrixU(); 


33 Eigen::Matrix3d V = svd.matrixV(); 

34 cout<<"U="<<U<<endl; 

35 cout<<"V="<<V<<endl; 

36 

37 Eigen: :Matrix3d R_ = U*(V.transpose()); 

38 Eigen::Vector3d t_ = Eigen::Vector3d( p1.x, p1.y, pl.z ) - R_ * Eigen::Vector3d 


( p2 ey Py; p22 )5 


40 // convert to cu::Mat 

41 R = ( Mat_<double>(3,3) << 

42 R_(0,0), R_(0,1), R_(0,2), 

43 R10): Re Cbs) 5 R52): 

44 R_(2,0), R_(2,1), R_(2,2) 

45 J3 

46 t = ( Mat_<double>(3,1) << t_(0,0), t_(1,0), t_(2,0) ) ; 


ICP 的 实现 方式 和 前 文 讲 述 的 是 一 致 的 。 我 们 调用 Eigen 进 行 SVD， 
然后 计算 R,t 和 矩 阵 。 我 们 输出 了 匹配 后 的 结果 ， 不 过 请 注意 ， 由 于 前 面 的 
推导 是 按照 p, =Rp , +t 进行 的 ， 这 里 的 R,t 是 第 二 帧 到 第 一 帧 的 变换 ， 与 前 
面 PnP 部 分 是 相反 的 。 所 以 在 输出 结果 中 ， 我 们 同时 打印 了 逆 变 换 : 


1 |% build/pose_estimation_3d3d 1.png 2.png 1_depth.png 2_depth.png 
2 |-- Max dist : 95.000000 
3 |-- Min dist : 4.000000 
4 | 一 共 找到 了 79 组 匹配 点 
3d-3d pairs: 74 
6 |W= 298.51 -14.1815 41.0456 
7 |-44.8208 107.825 -164.404 
s | 78.1978 -163.954 271.439 
9 |U= 0.474143 -0.880373 -0.0114952 
10 |-0.460275 -0.258979 0.849163 
u |0.750556 0.397334 0.528006 
2 |V= 0.535211 -0.844064 -0.0332488 
3 |-0.434767 -0.309001 0.84587 
14 |0.724242 0.438263 0.532352 
is | ICP via SVD results: 
16 |R = [0.9972395976914055, 0.05617039049497474, -0.04855998381307948; 
17 | -0.05598344580804095, 0.9984181433274515, 0.005202390798842771 ; 
18 | 0.04877538920134394, -0.002469474885032297, 0.998806719591959] 
19 |t = [0.7086246277241892; 
20 | -0.2775515782948791; 
2 | -0.1559573762377209] 
2 |R_inv = [0.9972395976914055, -0.05598344580804095, 0.04877538920134394; 


23 | 0.05617039049497474, 0.9984181433274515, -0.002469474885032297 ; 
24 | -0.04855998381307948, 0.005202390798842771, 0.998806719591959] 
2 |t_inv = [-0.7145999506834847 ; 

26 | 0.2369236766013986 ; 

27 | 0.1916260075851286] 


读者 可 以 比较 一 下 ICP 与 PnP、 对 极 几何 的 运动 估计 结果 之 间 的 差 
异 。 可 以 认为 ， 在 这 个 过 程 中 我 们 使 用 了 越 来 越 多 的 信息 (没有 深度 一 
有 一 个 图 的 深度 一 有 两 个 图 的 深度 ) ， 因 此 ， 在 深度 准确 的 情况 下 ， 得 
到 的 估计 也 将 越 来 越 准确 。 但 是 ， 由 于 Kinect 的 深 度 图 存在 噪声 ， 而 且 有 
可 能 存在 数据 丢失 的 情况 ， 使 得 我 们 不 得 不 丢弃 一 些 没有 深度 数据 的 特 
征 点 。 这 可 能 导致 ICP 的 估计 不 够 准确 ， 并 且 ， 如 果 特 征 点 丢弃 得 太 多 ， 
可 能 引起 由 于 特征 点 太 少 ， 无 法 进行 运动 估计 的 情况 。 


7.10.2 ” 非 线 性 优化 方法 


下 面 考 虑 用 非 线性 优化 来 计算 ICP。 我 们 依然 使 用 李 代 数 来 表达 相机 
位 次。 与 SVD 思 路 不 同 的 地 方 在 于 ， 在 优化 中 我 们 不 仅 考 虑 相机 的 位 
资 ， 同 时 会 优化 3D 扣 的 空间 位 置 。 对 我 们 来 襄 ，RGB-D 相 机 每 次 可 以 观 
测 到 路 标点 的 三 维 位 置 ， 从 而 产生 一 个 3D 观 测 数 据 。 不 过 ， 由 于 g20/sba 
中 没有 提供 3D 到 3D 的 边 ， 而 我 们 又 想 使 用 g20/sba 中 李 代 数 实现 的 位 姿 节 
点 ， 所 以 最 好 的 方式 是 自 定义 一 种 这 样 的 边 ， 并 向 g20 提 供 解析 求 导 方 
式 。 


内 slambook/ch7/pose_estimation_3d3d.cpp 


1 |class EdgeProjectXYZRGBDPoseOnly : public g20::BaseUnaryEdge<3, Eigen: :Vector3d, 
g2o: : VertexSE3Expmap> 

2 |{ 

3 | public: 

4 EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 

5 EdgeProjectXYZRGBDPoseOnly( const Eigen::Vector3d& point ) : 

6 _point(point) {} 

7 

8 virtual void computeError() 

9 3! 

10 const g20::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> 

( _vertices[0] ); 

ll // measurement is p, point is p' 

12 _error = _measurement - pose->estimate().map( _point ); 

13 } 

14 

15 virtual void linearize0plus() 

16 { 

17 g20::VertexSE3Expmap* pose = static_cast<g20::VertexSE3Expmap *>(_vertices 


[0]); 


18 g20::SE3Quat T(pose->estimate()); 
19 Eigen: :Vector3d xyz_trans = T.map(_point); 
20 double x = xyz_trans [0]; 

21 double y = xyz_trans[1]; 

2 double z = xyz_trans[2]; 

23 

24 _jacobian0plusXi(0,0) = 0; 

25 _jacobian0plusXi(0,1) = -z; 

26 _jacobian0plusxi(0,2) = y; 

27 _jacobian0plusxi(0,3) = -1; 

28 _jacobian0plusxi(0,4) = 0; 

29 _jacobian0plusxi(0,5) = 0; 

30 

31 _jacobian0plusxi(1,0) = z; 

32 _jacobian0plusXi(1,1) = 0; 

33 _jacobian0OplusXi(1,2) = -x; 

34 _jacobian0plusxi(1,3) = 0; 

35 _jacobian0Oplusxi(1,4) = -1; 

36 _jacobian0plusxi(1,5) = 0; 

37 

38 _jacobian0plusxi(2,0) = -y; 

39 _jacobianOplusxi(2,1) = x; 

40 _jacobian0plusxi(2,2) = 0; 

41 _jacobian0plusXi (2,3) = 0; 

42 _jacobian0plusXi(2,4) = 0; 

43 _jacobian0plusXi(2,5) = -1; 

44 

45 

46 bool read ( istream& in ) {} 

47 bool write ( ostream& out ) const {} 


48 | protected: 


49 Eigen::Vector3d _point; 
so |}; 
这 是 一 个 一 元 边 ， 写 法 类 似 于 前 面 提 到 的 g2o::EdgeSE3ProjectXYZ A 


T 4 观测 量 从 2 维 变 成 了 3 维 ， 内 部 没有 相机 模型 ， 并 且 只 关联 到 一 个 节 
点 。 请 读者 注意 这 里 雅 可 比 矩 阵 的 书写 ， 己 必 须 与 我 们 前 面 的 推导 一 
致 。 雅 可 比 窍 阵 给 出 了 关于 相机 位 资 的 导数 ， 是 一 个 3x 6 的 矩阵 。 


调用 g2o 进 行 优 化 的 代码 是 相似 的 ， 我 们 设 定好 图 优化 的 节点 和 边 即 
可 。 这 部 分 代码 请 读者 查看 产 文 件 ， 这 里 不 再 列 出 。 现 在 ， 来 看 看 优化 
的 结果 : 


1 | calling bundle adjustment 
2 | iteration= 0 chi2= 452884.696837 time= 3.8443e-05 cumTime= 3.8443e-05 edges= 74 


schur= 0 

3 | iteration= 1 chi2= 452762.638918 time= 1.436e-05 cumTime= 5.2803e-05 edges= 74 
schur= 0 

4 |iteration= 2 chi2= 452762.618632 time= 1.1943e-05 

由 中 间 略 

6 |iteration= 9 chi2= 452762.618615 time= 1.0772e-05 cumTime= 0.000140108 edges= 74 
schur= 0 


7 | optimization costs time: 0.000528066 seconds. 


9 |after optimization: 

10 | T= 

u |0.99724 0.0561704 -0.04856 0.708625 

2 | -0.0559834 0.998418 0.00520239 -0.277551 
3 | 0.0487754 -0.00246948 0.998807 -0.155957 
4 |O 0 0 1 


我 们 发 现 ， 只 迭代 一 次 后 总 体 误差 就 已 经 稳定 不 变 ， 说 明 仅 在 一 次 
和 迭代 之 后 算法 即 已 收 你 。 从 位 资 求 解 的 结果 可 以 看 出 ， 它 和 前 面 SVD 给 
出 的 位 姿 结 果 几 乎 一 模 一 样 ， 这 说 明 SVD 已 经 给 出 了 优化 问题 的 解析 
解 。 所 以 ， 本 实验 中 可 以 认为 SVD 给 出 的 结果 是 相机 位 姿 的 最 优 值 。 


需要 说 明 的 是 ， 在 本 例 的 ICP 中 ， 我 们 使 用 了 在 两 个 图 都 有 深度 读数 
的 特征 点 。 然 而 事实 上 ， 只 要 其 中 一 个 图 深度 确定 ， 我 们 就 能 用 类 似 于 
PnP 的 误差 方式 ， 把 它们 也 加 到 优化 中 来 。 同 时 ， 除 了 相机 位 姿 之 外 ， 将 
空间 点 也 作为 优化 变量 考虑 ， 亦 是 一 种 解决 问题 的 方式 。 我 们 应 当 清 
楚 ， 实 际 的 求解 是 非常 灵活 的 ， 不 必 拘 泥 于 某 种 固定 的 形式 。 如 果 同 时 
考虑 点 和 相机 ， 整 个 问题 就 变 得 更 自由 了 ， 你 可 能 会 得 到 其 他 的 解 。 比 
如 ， 可 以 让 相机 少 转 一 些 角度 ， 而 把 点 多 移动 一 些 。 这 从 另 一 侧面 反映 
出 ， 在 Bundle Adjustment 里面， 我 们 会 希望 有 尽 可 能 多 的 约束 ， 因 为 多 次 
观测 会 带 来 更 多 的 信息 ， 使 我 们 能 够 更 准确 地 估计 每 个 变量 。 


7.11 小结 


本 讲 介绍 了 基于 特征 点 的 视觉 里 程 计 中 的 几 个 重要 的 问题 。 包 括 : 

1. 特 征 点 是 如 何 提取 并 匹配 的 。 

2. 如 何 通 过 2D-2D 的 特征 点 估计 相机 运动 。 

3. 如 何 从 2D-2D 的 匹配 估计 一 个 点 的 空间 位 置 。 

4.3D-2D 的 PnP 问题 ， 其 线性 解法 和 Bundle Adjustment 解 法 。 

5.3D-3D 的 ICP 问 题 ， 其 线性 解法 和 Bundle Adjustment 解 法 。 

本 讲 内 容 较 为 丰富 ， 且 结合 应 用 了 前 几 讲 的 基本 知识 。 读 者 若 觉 得 
理解 有 困难 ， 可 以 对 前 面 知 识 稍 加 回顾 。 最 好 杀 自 做 一 损 实 验 ， 以 理解 
整个 运动 估计 的 内 容 。 

需要 解释 的 是 ， 为 保证 行文 流畅 ， 我 们 省 略 了 大 量 关 于 某 些 特殊 情 
况 的 讨论 。 例 如 ， 如 果 在 对 极 几 何 求解 过 程 中 给 定 的 特征 点 共 面 ， 会 发 
生 什 么 情况 〈 这 在 单 应 和 矩阵 万 中 提 到 了 ) ? 共 线 又 会 发 生 什么 情况 ? 在 
PnP 和 ICP 中 知 给 定 这 样 的 解 ， 又 会 导致 什么 情况 ?求解 算法 能 否 识别 这 
些 特殊 的 情况 ， 并 报告 所 得 的 解 可 能 不 可 靠 ? 尽管 它们 都 是 值得 研究 和 
探索 的 ， 然 而 对 它们 的 讨论 势必 让 本 书 变 得 特别 烦琐 。 而 且 在 工程 实现 
中 ， 这 些 情况 甚 少 出 现 ， 所 以 本 书 介绍 的 方法 ， 是 指 在 实际 工程 中 能 够 
有 效 运行 的 方法 ， 我 们 假定 了 那些 少见 的 情况 并 不 发 生 。 如 果 你 关心 这 
些 少见 的 情况 ， 可 以 阅读 [3] 等 论文 ， 在 文献 中 我 们 经 常会 研究 一 些 特 殊 
情况 下 的 解决 方案 。 

习题 

1. 除 了 本 书 介 绍 的 ORB 特 征 点 外 ， 你 还 能 找到 哪些 特征 点 ?请 说 说 
SIFT 或 SURF 的 原理 ， 并 对 比 它们 与 ORB 之 间 的 优 劣 。 


2. 设 计 程 序 调用 OpenCV 中 的 其 他 种 类 特征 点 。 统 计 在 提取 1000 个 特 
征 点 时 在 你 的 机 器 上 所 用 的 时 间 。 

3.* 我 们 发 现 ，OpenCV 提 供 的 ORB 特 征 点 在 图 像 当中 分 布 不 够 均匀 。 
你 是 否 能 够 找到 或 提出 让 特征 点 分 布 更 加 均匀 的 方法 ? 

4. 研 究 FLANN 为 何 能 够 快速 处 理 匹 配 问题 。 除 了 FLANN 之 外 ， 还 有 
哪些 可 以 加 速 匹 配 的 手段 ? 


5. 把 演示 程序 使 用 的 EPnP 改 成 其 他 PnP 方法 ， 并 研究 它们 的 工作 原 
理 。 


6. 在 PnP 优化 中 ， 将 第 一 个 相机 的 观测 也 考虑 进来 ， 程 序 应 如 何 书 
写 ? 最 后 结果 会 有 何 变化 ? 


将 空间 点 也 作为 优化 变量 考虑 进来 ， 程 序 应 如 何 书 
最 后 结果 会 有 何 变化 ? 


8.* 在 特征 点 匹配 过 程 中 ， 不 可 避免 地 会 遇 到 误 匹 配 的 情况 。 如 果 我 
们 把 错误 匹配 输入 到 PnP 或 CP 中 ， 会 发 生 怎 样 的 情况 ?你 能 想到 哪些 避 
免 误 匹配 的 方法 ? 


9.* 使 用 Sophus 的 SE3 类 ， 自 己 设计 g20 的 节点 与 边 ， 实 现 PnP 和 ICP 的 
优化 。 


10.* 在 Ceres 中 实现 PnP 和 ICP 的 优化 。 


[1] 金字 塔 是 指 对 图 像 进行 不 同 层次 的 降 采 样 ， 以 获得 不 同 分 辨 率 的 图 像 。 
[2] 也 就 是 说 ， 在 等 式 一 侧 乘 以 任意 非 零 常数 时 ， 我 们 认为 等 式 仍 是 成 立 的 。 
[3] 在 SftM 研 究 中 则 有 可 能 是 未 知 而 有 待 估 计 的 。 

[4] 请 注意 ， 这 和 SE(3) 中 的 变换 矩阵 了 是 不 同 的 。 


[5] exp(E^)P ,结果 是 4x 1 的 ， 而 其 左 侧 的 K 是 3x 3 的 ， 所 以 必须 把 exp(E^)P ,的 前 三 维 取 出 来 ， 
变 成 三 维 的 非 齐 次 坐标 。 这 在 前 面 讲 过 。 
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主要 目标 

1. 理 解 光 流 法 跟踪 特征 点 的 原理 。 

2. 理 解 直接 法 是 如 何 估计 相机 位 资 的 。 
3. 使 用 g2o 进 行 直接 法 的 计算 。 


直接 法 是 视觉 里 程 计 另 一 主要 分 支 ， 它 与 特征 点 法 有 很 大 不 同 。 虽 
然 它 还 没有 成 为 现在 VO 中 的 主流 ， 但 经 过 近 几 年 的 发 展 ， 直 接 法 在 一 定 
程度 上 已 经 能 和 特征 点 法 平分 秋色 。 本 讲 我 们 将 介绍 直接 法 的 原理 ， 并 
利用 g2o 实 现 直接 法 中 的 一 些 核心 算法 。 
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8.1 直接 法 的 引出 


上 一 讲 我 们 介绍 了 使 用 特征 点 估计 相机 运动 的 方法 。 尽 管 特征 点 法 


在 视觉 里 程 计 中 占据 主流 地 位 ， 但 研究 者 们 还 是 认识 到 它 至 少 有 以 下 几 
个 缺点 : 


1. 关 键 点 的 提取 与 描述 子 的 计算 非常 耗 时 。 实 践 当 中 ，SIFT 目 前 在 
CPU 上 是 无 法 实时 计算 的 ， 而 ORB 也 需要 近 20ms 的 计算 。 如 果 整 个 
SLAM 以 30 毫 秒 / 帧 的 速度 运行 ， 那 么 一 大 半 时 间 都 将 花 在 计算 特征 点 
Bs 


2. 使 用 特征 点 时 ， 忽 略 了 除 特征 点 以 外 的 所 有 信息 。 一 幅 图 像 有 几 十 
万 个 像素 ， 而 特征 点 只 有 几 百 个 。 只 使 用 特征 点 丢弃 了 大 部 分 可 能 有 用 
的 图像 信息 。 


3. 相 机 有 时 会 运动 到 特征 缺失 的 地 方 ， 这 些 地 方 往往 没有 明显 的 纹 
理 信息 。 例 如 ， 有 时 我 们 会 面 对 一 堵 白 墙 ， 或 者 一 个 空荡荡 的 走廊 。 这 
些 场景 下 特征 后 数量 会 明显 减少 ， 我 们 可 能 找 不 到 足够 的 匹配 点 来 计算 
相机 运动 。 


我 们 看 到 使 用 特征 氮 确实 存在 一 些 问题 。 有 没有 什么 办 法 能 够 克服 
这 些 缺 点 呢 ? 我 们 有 以 下 几 种 思路 : 


“保留 特征 点 ， 但 只 计算 关键 点 ， 不 计算 描述 子 。 同 时 ， 使 用 光 流 法 
(OpticalFlow) 来 跟踪 特征 点 的 运动 。 这 样 可 以 回避 计算 和 匹配 描述 子 
带 来 的 时 间 ， 但 光 流 本 身 的 计算 需要 一 定时 间 。 


“只 计算 关键 点 ， 不 计算 描述 子 。 同时 ， 使 用 直接 法 ( Direct 
Method) 来 计算 特征 点 在 下 一 时 刻 图 像 中 的 位 置 。 这 同样 可 以 跳 过 描述 
子 的 计算 过 程 ， 而 且 直 接 法 的 计算 更 加 简单 。 

“ 既 不 计算 关键 点 ， 也 不 计算 描述 子 ， 而 是 根据 像素 灰 度 的 差异 ， 直 
接 计算 相机 运动 。 


第 一 种 方法 仍然 使 用 特征 点 ， 只 是 把 匹配 描述 子 蔡 换 成 了 光 流 跟 
足 ， 估 计 相 机 运动 时 仍 使 用 对 极 几何 、PnP 或 ICP 算 法 。 而 在 后 两 种 方法 
我 们 会 根据 图 像 的 像素 灰 度 信息 来 计算 相机 运动 ， 它 们 都 称 为 直接 


使 用 特征 点 法 估计 相机 运动 时 ， 我 们 把 特征 点 看 作 固定 在 三 维 空间 
的 不 动 点 。 根 据 它们 在 相机 中 的 投影 位 置 ， 通 过 最 小 化 重 投影 误差 
(Reprojection error) 来 优化 相机 运动 。 在 这 个 过 程 中 ， 我 们 需要 精确 地 
知道 空间 点 在 两 个 相机 中 投影 后 的 像素 位 置 一 一 这 也 就 是 我 们 要 对 特征 
进行 匹配 或 跟踪 的 原因 。 同 时 ， 我 们 也 知道 ， 计 算 、 匹 配 特 征 需 要 付出 
大 量 的 计算 量 。 相 对 地 ， 在 直接 法 中 ， 我 们 并 不 需要 知道 点 与 点 之 间 的 
对 应 关系 ， 而 是 通过 最 小 化 光度 误差 (Photometric error) 来 求 得 它们 。 


直接 法 是 本 讲 介绍 的 重点 。 它 是 为 了 克服 特征 点 法 的 上 述 缺点 而 存 
在 的 。 直 接 法 根据 像素 的 亮度 信息 估计 相机 的 运动 ， 可 以 完全 不 用 计算 
关键 点 和 描述 子 ， 于 是 ， 既 避免 了 特征 的 计算 时 间 ， 也 避免 了 特征 缺失 
的 情况 。 只 要 场景 中 存在 明暗 变化 (可 以 是 渐变 ， 不 形成 局 部 的 图 像 梯 
E) ， 直 接 法 就 能 工作 。 根 据 使 用 像素 的 数量 ， 直 接 法 分 为 稀 玻 、 稠 密 
和 半 稠 密 三 种 。 相 比 于 特征 点 法 只 能 重 构 稀 玻 特 征 点 (HHA) ， 直 
接 法 还 具有 恢复 稠密 或 半 稠 密 结构 的 能 力 。 


历史 上 ， 早 期 也 有 对 直接 法 的 使 用 I。 随 着 一 些 使 用 直接 法 的 开源 
项 目的 出 现 (如 SVOS9 、LSD-SLAMI5 等 ) ， 它 们 逐渐 地 走 上 主流 舞 
台 ， 成 为 视觉 里 程 计算 法 中 重要 的 一 部 分 。 


8.2” 光 流 (OpticalFlow) 


直接 法 是 从 光 流 演变 而 来 的 。 它 们 非常 相似 ， 具 有 相同 的 假设 条 
件 。 光 流 描述 了 像素 在 图 像 中 的 运动 ， 而 直接 法 则 附带 着 一 个 相机 运动 
模型 。 为 了 说 明 直 接 法 ， 我 们 先 来 介绍 一 下 光 流 。 

光 流 是 一 种 摘 述 像素 随时 间 在 图 像 之 间 运 动 的 方法 ， 如 图 8-1 所 示 。 
随 着 时 间 的 流逝 ， 同 一 个 像素 会 在 图 像 中 运动 ， 而 我 们 希望 妃 踪 它 的 运 
动 过 程 。 其 中 ， 计 算 部 分 像素 运动 的 称 为 稀疏 光 流 ， 计 算 所 有 像素 的 称 
为 稠密 光 流 。 稀 疏 光 流 以 Lucas-Kanade 光 流 为 代表 ， 并 可 以 在 SLAM 中 用 
于 跟踪 特征 点 位 置 。 因 此 ， 本 节 主 要 介绍 Lucas-Kanade 光 流 ， 亦 称 LK 光 


Re aN 
WILo 


灰 度 不 变 假设 : IT (x, Yt ) = 1 (X25 yb)=T(%,»,6) 
图 8-1 LK 光 流 法 示意 图 。 


Lucas-Kanade 光 流 


在 LK 交流 中 ， 我 们 认为 来 自 相机 的 图 像 是 随时 间 变 化 的 。 图 像 可 以 
看 作 时 间 的 阔 效 : T(t)。 那 么 ， 一 个 在 t 时刻， 位 于 (xy ) 处 的 像素 ， 它 的 
灰 度 可 以 写成 


I(x, y,t). 


这 种 方式 把 图 像 看 成 了 关于 位 置 与 时 间 的 函数 ， 它 的 值 域 就 是 图 像 
中 像素 的 灰 度 。 现 在 考虑 某 个 固定 的 空间 点 ， 它 在 ! 时 刻 的 像素 坐标 为 x,y 
。 由 于 相机 的 运动 ， 它 的 图 像 坐 标 将 发 生变 化 。 我 们 希望 估计 这 个 空间 
点 在 其 他 时 刻 图 像 中 位 置 。 怎 么 估计 呢 ? 这 里 要 引入 光 流 法 的 基本 假 
Io 

灰 度 不 变 假设 : 同一 个 空间 点 的 像素 灰 度 值 ， 在 各 个 图 像 中 是 固定 
不 变 的 。 


对 于 t 时 刻 位 于 (xy ) 处 的 像素 ， 我 们 设 t +dt 时 刻 它 运动 到 (x +dx,y +dy 
) 处 。 由 于 灰 度 不 变 ， 我 们 有 : 


I(a+dz,y+dy,t+dt) = I(a,y,t). (8.1) 


灰 度 不 变 假设 是 一 个 很 强 的 假设 ， 实 际 当 中 很 可 能 不 成 立 。 事 实 
上 ， 由 于 物体 的 材质 不 同 ， 像 素 会 出 现 高 光 和 阴影 部 分 ; 有时， 相机 会 
目 动 调整 曝光 参数 ， 使 得 图 像 整 体 变 亮 或 变 瞳 。 这 些 时 候 灰 度 不 变 假设 
都 是 不 成 立 的 ， 因 此 光 流 的 结果 也 不 一 定 可 靠 。 然 而 ， 从 另 一 方面 来 
说 ， 所 有 算法 都 是 在 一 定 假设 下 工作 的 。 如 果 我 们 什么 假设 都 不 做 ， 就 


没 法 设计 实用 的 算法 。 所 以 ， 让 我 们 暂且 认为 该 假设 成 立 ， 看 看 如 何 计 
算 像 素 的 运动 。 
I(2+dz,y+dy,t+dt) ~ I (x,y, t) + or ae + Say + ĉl at, (8.2) 
因为 我 们 假设 了 灰 度 不 变 ， 于 是 下 一 个 时 刻 的 灰 度 等 于 之 前 的 灰 
度 ， 从 而 : 
Or Or ðI 


agi + Iy” + at 一 0. (8.3) 


两 边 除 以 dt ， 得 : 


OI dx Idy _ ol 
Ox dt Oydt 6 


(8.4) 


其 中 dx /di 为 像素 在 x 轴 上 的 运动 速度 ， 而 dy /dt Ay 轴 上 的 速度 ， 把 
它们 记 为 uv 。 同 时 01 /0x 为 图 像 在 该 点 处 x 方向 的 梯度 ， 另 一 项 则 是 在 y 
方向 的 梯度 ， 记 为 I ,1, 。 把 图 像 灰 度 对 时 间 的 变化 量 记 为 I ， 写 成 矩阵 形 


式 ， 有 : 


|z n J| | =- (8.5) 


我 们 想 计算 的 是 像素 的 运动 u,v ， 但 是 该 式 是 带 有 两 个 变量 的 一 次 方 
程 ， 仅 凭 它 无 法 计算 出 wv。 因 此， 必须 引入 额外 的 约束 来 计算 u,v 。 在 
LK 光 流 中 ， 我 们 假设 某 一 个 窗口 内 的 像素 具有 相同 的 运动 。 

考虑 一 个 大 小 为 wxw 的 窗口 ， 它 含有 w : 数量 的 像素 。 由 于 该 窗口 内 
像素 具有 同样 的 运动 ， 因 此 我 们 共有 w ?个 方程 : 


| 五 SAH seige baben. (8.6) 


记 : 
[Ie Tl In 
A= i ,b= : (8.7) 
(IL, 了 Iik 
于 是 整个 方程 为 
u 
A = —b. (8.8) 
U 


是 一 个 天 于 u,v 的 超 定 线性 方程 ， 传 统 解法 是 求 最 小 二 乘 解 。 最 小 
二 乘 在 很 多 时 候 都 用 到 过 : 


| . | = —(A™A) AD. (8.9) 


V 


这 样 就 得 到 了 像素 在 图 像 间 的 运动 速度 u,v 。 当 t 取 离散 的 时 刻 而 不 
是 连续 时 间 时 ， 我 们 可 以 估计 某 块 像素 在 若干 个 图 像 中 出 现 的 位 置 。 由 
于 像素 梯度 仅 在 局 部 有 效 ， 所 以 如 果 一 次 迭代 不 够 好 ， 我 们 会 多 迭代 几 
次 这 个 方程 。 a LK 光 流 常 被 用 来 跟踪 角 点 的 运动 ， 我 们 不 妨 
通过 程序 体会 一 下 。 


8.3 XE: LK 光 流 


8.3.1 ”使 用 TUM 公 开 数 据 集 


下 面 演 示 如 何 用 OpenCV 提 供 的 光 流 法 来 跟踪 特征 点 。 与 上 一 节 一 
样 ， 我 们 准备 了 若干 幅 数 据 集 图 像 ， 存 放 在 程序 目录 中 的 data/ 文 件 夹 
下 。 它 们 来 自 于 慕尼黑 工业 大 学 (TUM) 提供 的 公开 RGB-D 数 据 集 D 。 
以 后 我 们 就 称 之 为 TUM 数 据 集 。 它 含有 许多 个 RGB-D 视 频 ， 可 以 作为 
RGB-D 或 单 目 SLAM 的 实验 数据 。 它 还 提供 了 用 运动 捕捉 系统 测量 的 精 
确 轨迹 ， 可 以 作为 标准 轨迹 以 校准 SLAM 系 统 。 由 于 该 数据 集 比较 大 ， 我 
们 没有 放 到 GitHub 上 (否则 下 载 代码 需要 很 长 时 间 ) ， 请 读者 自行 去 数 
据 集 主页 查找 对 应 的 数据 。 本 程序 中 使 用 了 一 部 分 “freburg1_desk” 数 据 集 
中 的 图 像 。 读 者 可 以 在 TUM 数 据 集 主页 找到 它 的 下 载 链接 。 或 者 ， 也 可 
以 直接 使 用 本 书 在 GitHub 上 提供 的 部 分 。 


我 们 的 数据 位 于 本 章 目 录 的 data/ 下， 以 压缩 包 形 式 提 供 
(data.tar.gz) 。 由 于 TUM 数 据 集 是 从 实际 环境 中 采集 的 ， 需 要 解释 一 下 
它 的 数据 格式 〈 数 据 集 一 般 都 有 自己 定义 的 格式 ) 。 在 解压 后 ， 你 将 看 
到 以 下 这 些 文 件 : 
1.rgb.txt 和 depth.txt 记 录 了 各 文件 的 采集 时 间 和 对 应 的 文件 名 。 


2.rgb/ 和 depth/ 目 录 存 放 着 采集 到 的 PNG 格 式 图 像 文 件 。 彩 色 图 像 为 8 
位 3 通道 ， 深 度 图 为 16 位 单 通道 图 像 。 文 件 名 即 采 集 时 间 。 


3.groundtruth.txt 为 外 部 运动 捕捉 系统 采集 到 的 相机 位 姿 ， 格 式 为 
(time, tz, ty, tz, qz, dy, 4z; qw), 


我 们 可 以 把 它 看 成 标准 轨迹 。 


请 注意 ， 彩 色 图 、 深 度 图 和 标准 轨迹 的 采集 都 是 独立 的 ， 轨 迹 的 采 
集 频 率 比 图 像 高 很 多 。 在 使 用 数据 之 前 ， 需 要 根据 采集 时 间 对 数据 进行 
一 次 时 间 上 的 对 齐 ， 以 便 对 彩色 图 和 深度 图 进行 配对 。 原 则 上 ， 我 们 可 
以 把 采集 时 间 相 近 于 一 个 阐 值 的 数据 ， 看 成 是 一 对 图 像 。 并 把 相近 时 间 
的 位 姿 ， 看 作 是 该 图 像 的 真实 采集 位 置 。TUM 提 供 了 一 个 Python 脚 本 
“associate.py”( 或 使 用 slambook/tools/associate.py) 帮 有 我 们 完成 这 件 事 。 
请 把 此 文件 放 到 数据 集 目录 下 ， 运 行 : 


1 |python associate.py rgb.txt depth.txt > associate.txt | 


这 段 脚 本 会 根据 输入 的 两 个 文件 中 的 采集 时 间 进 行 配对 ， 最 后 输出 
到 文件 associate.txt。 输 出 文件 含有 配对 后 的 两 幅 图 像 的 时 间 、 文 件 名 信 
息 ， 可 以 作为 后 续 处 理 的 来 源 。 此 外 ，TUM 数 据 集 还 提供 了 比较 估计 轨 
迹 与 标准 轨迹 的 工具 ， 我 们 将 在 用 到 的 地 方 再 进行 介绍 。 


8.3.2 ”使 用 LK 光 流 


下 面 来 编写 程序 使 用 OpenCV 中 的 LK 光 流 。 使 用 LK 的 目的 是 跟踪 特 
征 点 。 我 们 对 第 一 幅 图 像 提取 FAST 角 点 ， 然 后 用 LK 光 流 跟踪 它们 ， 并 画 
在 图 中 。 


kÀ slambook/ch8/useLK/useLK.cpp 


1 | #include <iostream> 
2 |#include <fstream> 
3 | #include <list> 

4 |#include <vector> 

5 | #include <chrono> 


6 |using namespace std; 


#include <opencv2/core/core.hpp> 
#include <opencv2/highgui/highgui .hpp> 
#include <opencv2/features2d/features2d.hpp> 


#include <opencv2/video/tracking.hpp> 


int main( int argc, char** argv ) 


{ 
if ( argc != 2 ) 
{ 
cout<<"usage: useLK path_to_dataset"<<end1; 
return 1; 
} 


string path_to_dataset = argv[1]; 
string associate_file = path_to_dataset + "/associate.txt"; 
ifstream fin( associate_file ); 
string rgb_file, depth_file, time_rgb, time_depth; 
list< cv::Point2f > keypoints; // 因为 要 删除 跟踪 失败 的 点 ， 
cv::Mat color, depth, last_color; 
for ( int index=0; index<100; index++ ) 
{ 
fin>>time_rgb>>rgb_file>>time_depth>>depth_file; 
color = cv::imread( path_to_datasett+"/"+rgb_file ); 
depth = cv::imread( path_to_dataset+"/"+depth_file, -1 ); 
if (index ==0 ) 
{ 
// 对 第 一 帧 提取 FAST 特征 点 


vector<cv::KeyPoint> kps; 


cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector:: 


create(); 
detector->detect( color, kps ); 
for ( auto kp:kps ) 
keypoints.push_back( kp.pt ); 
last_color = color; 
continue; 
} 
if ( color.data==nullptr || depth.data==nullptr ) 
continue; 
// 对 其 他 帧 用 LK 跟踪 特征 点 
vector<cv::Point2f> next_keypoints; 
vector<cv::Point2f> prev_keypoints; 
for ( auto kp:keypoints ) 
prev_keypoints.push_back (kp); 
vector<unsigned char> status; 
vector<float> error; 


chrono::steady_clock::time_point tl = chrono::steady_clock 


使 用 List 


: :now(); 


52 


cv::calcOpticalFlowPyrLK( last_color, color, prev_keypoints, next_keypoints 


; Status, error. ); 


chrono: :steady_clock::time_point t2 = chrono::steady_clock: :now(); 


chrono: :duration<double> time_used = chrono: :duration_cast<chrono: :duration 


<double>>( t2-t1 ); 


cout<<"LK Flow use time: "<<time_used.count()<<" seconds."<<end1; 
// 把 跟 丢 的 点 删 掉 


int i=0; 


for ( auto iter=keypoints.begin(); iter!=keypoints.end(); i++) 


{ 


} 


if ( status[i] == 0 ) 

{ 
iter = keypoints.erase(iter) ; 
continue; 

} 

xiter = next_keypoints [i] ; 


itert++; 


cout<<"tracked keypoints: "<<keypoints.size()<<end1; 


if (keypoints.size() == 0) 


4 


} 


// 


cout<<"all keypoints are lost."<<end1; 


break; 


3 keypoints 


cv::Mat img_show = color.clone(); 


for ( auto kp:keypoints ) 


cv::circle(img_show, kp, 10, cv::Scalar(0, 240, 0), 1); 


cv::imshow("corners", img_show) ; 


cv::waitKey (0); 


last_color = color; 


81 } 
82 return 0; 
33 |} 


读者 应 当 已 经 熟悉 了 OpenCV 的 使 用 方式 ， 这 
CMakeLists.txt 的 写法 了 。 该 程序 的 运行 参数 里 需要 指定 数据 集 所 在 的 目 


KR PIAN: 


里 就 不 提供 


./build/useLK ../data | 


我 们 会 在 每 次 循环 后 暂停 程序 ， 按 任意 键 可 以 继续 运行 。 你 会 看 到 
图 像 中 大 部 分 特征 点 都 能 够 顺利 跟踪 到 ， 但 也 有 特征 点 会 丢失 。 丢 失 的 
特征 点 或 是 被 移出 了 视野 外 ， 或 是 被 其 他 物体 挡住 了 。 如 果 我 们 不 提取 
新 的 特征 点 ， 那 么 光 流 的 跟踪 会 越 来 越 少 : 


1 |% build/useLK ../data 
2 |LK Flow use time: 0.0329535 seconds. 
3 | tracked keypoints: 1749 
4 |LK Flow use time: 0.0247758 seconds. 
tracked keypoints: 1742 
6 |LK Flow use time: 0.0226143 seconds. 
7 | tracked keypoints: 1703 
s |LK Flow use time: 0.0238692 seconds. 
9 | tracked keypoints: 1676 
10 | LK Flow use time: 0.0210466 seconds. 
1l1 |tracked keypoints: 1664 
122 |LK Flow use time: 0.0226533 seconds. 
13 | tracked keypoints: 1656 
14 | LK Flow use time: 0.0266527 seconds. 
15 | tracked keypoints: 1641 
16 |LK Flow use time: 0.0214207 seconds. 
17 | tracked keypoints: 1634 


图 8-2 显 示 了 程序 运行 过 程 中 若干 帧 的 情况 《这 里 使 用 了 完整 的 数据 
集 ， 但 本 书 的 git 上 只 给 出 了 10 张 图 ) 。 最 初 我 们 大 约 有 1700 个 特征 点 。 
跟踪 过 程 中 一 部 分 特征 点 会 丢失 ， 直 到 100 帧 时 我 们 还 有 约 178 个 特征 
点 ， 相 机 视角 相对 于 最 初 的 图 像 也 发 生 了 较 大 改变 。 


图 8-2”LK 光 流 法 实验 。 


仔细 观察 特征 点 的 跟踪 过 程 ， 我 们 会 发 现 位 于 物体 角 点 处 的 特征 更 
加 稳定 。 边 缘 处 的 特征 会 沿 着 边缘 “滑动 ”， 这 主要 是 由 于 沿 着 边缘 移动 
时 特征 块 的 内 容 基 本 不 变 ， 因 此 程序 容易 认为 是 同一 个 地 方 。 而 既 不 在 
角 点 也 不 在 边缘 的 特征 点 则 会 频繁 跳动 ， 位 置 非 常 不 稳定 。 这 个 现象 很 
像 围棋 中 的 “ 金 角 银 边 草 肚皮 ”: 角 点 具有 更 好 的 辨识 度 ， 边 缘 次 之 ， 区 
块 最 少 。 


另 一 方面 ， 读 者 可 以 看 到 光 流 法 的 运行 时 间 。 在 跟踪 1500 个 特征 点 
时 ，LK 光 流 法 大 约 需要 20ms。 如 果 减 小 特征 点 的 效 量 ， 则 会 明显 减少 计 
算 时 间 。 我 们 看 到 ，LK 光 流 跟踪 法 避免 了 摘 述 子 的 计算 与 匹配 ， 但 本 身 
也 需要 一 定 的 计算 量 。 在 我 们 的 计算 平台 上 ， 使 用 LK 交流 能 够 节省 一 定 
的 计算 量 ， 但 在 具体 SLAM 系 统 中 ， 使 用 光 流 还 是 匹配 描述 子 ， 最 好 是 邓 
自 做 实验 测试 一 Fo 


另外 ，LK 光 流 跟踪 能 够 直接 得 到 特征 点 的 对 应 和 关系。 这 个 对 应 关系 
就 像 是 描述 子 的 匹配 ， 但 实际 上 我 们 大 多 数 时 候 只 会 磁 到 特征 点 跟 丢 的 
情况 ， 而 不 太 会 遇 到 误 匹 配 ， 这 应 该 是 光 流 相对 于 描述 子 的 一 点 优势 。 
但 是 ， 匹 配 描述 子 的 方法 在 相机 运动 较 大 时 仍 能 成 功 ， EL 
相机 运动 是 微小 的 。 从 这 方面 来 说 ， 光 流 的 健壮 性 比 描述 子 差 一 


最 后 ， 我 们 可 以 通过 光 流 跟踪 的 特征 点 ， 用 PnP、ICP 或 对 极 几何 来 
估计 相机 和 运动， 这 些 方法 在 上 一 讲 中 介绍 过 ， 这 里 不 再 讨论 。 总 而 言 
之 ， 光 流 法 可 以 加 速 基于 特征 点 的 视觉 里 程 计 算法 ， 避 免 计 算 和 匹配 搞 
述 子 的 过 程 ， 但 要 求 相 机 运动 较 慢 (或 采集 频率 较 高 )。 


8.4 直接 法 (DirectMethod) 


接 下 来 ， 我 们 来 讨论 与 光 流 有 一 定 相似 性 的 直接 法 。 与 前 面 内 容 相 
似 ， 我 们 先 介绍 直接 法 的 原理 ， 然 后 使 用 g2o 实 现 直接 法 。 


8.41 直接 法 的 推导 


如 图 8-3 所 示 ， 考 虑 东 个 空间 点 P 和 两 个 时 刻 的 相机 。P 的 世界 坐标 
为 [X,YZ ]， 它 在 两 个 相机 上 成 像 ， 记 非 齐 次 像素 坐标 为 p , ,p , 。 


我 们 的 目标 是 求 第 一 个 相机 到 第 二 个 相机 的 相对 位 瓷 变换 。 我 们 以 
第 一 个 相机 为 参照 系 ， 设 第 二 个 相机 的 旋转 和 平移 为 R,t 〈 对 应 李 代 数 为 E 
) 。 同 时 ， 两 相机 的 内 参 相同 ， 记 为 K 。 为 清楚 起 见 ， 我 们 列 写 完整 的 
投影 方程 : 


Rt 
第 1 帧 ^ 第 2 由 
LI exp(é*) 第 2 帧 


图 8-3 ”直接 法 示意 图 。 


1 1 
p2= | v | =K (RP +t) = 7K (exp (£^) P),.3. 
Ze Zs 


2 


其 中 Z , 是 P 的 深度 ，Z , 是 P 在 第 二 个 相机 坐标 系 下 的 深度 ， 也 就 是 
RP + 的 第 3 个 坐标 值 。 由 于 exp(&^ ) 只 能 和 齐 次 坐标 相 乘 ， 所 以 我 们 乘 完 
之 后 要 取出 前 3 个 元 素 。 这 和 上 一 讲 及 相机 模型 部 分 的 内 容 是 一 致 的 。 

回忆 特征 点 法 中 ， 由 于 我 们 通过 匹配 描述 子 知 道 了 p , ,p , 的 像素 位 


置 ， 所 以 可 以 计算 重 投影 的 位 置 。 但 在 直接 法 中 ， 由 于 没有 特征 匹配 ， 
我 们 无 从 知道 哪 一 个 p , Sp, 对 应 着 同一 个 点 。 直 接 法 的 思路 是 根据 当前 


相机 的 位 姿 估 计 值 来 寻找 p ,的 位 置 。 但 若 相 机 位 姿 不 够 好 ，p , 的 外 观 和 
p ,会 有 明显 差别 。 于 是 ， 为 了 减 小 这 个 差别 ， 我 们 优化 相机 的 位 姿 ， 来 
寻找 与 p , 更 相似 的 p , 。 这 同样 可 以 通过 解 一 个 优化 问题 完成 ， 但 此 时 最 
小 化 的 不 是 重 投影 误差 ， 而 是 光度 误差 (Photometric Error) ， 也 就 是 P 
的 两 个 像素 的 亮度 误差 : 
e 一 万 (pi) 一 了 2 (p2). (8.10) 

注意 这 里 e 是 一 个 标量 。 同 样 地 ， 优 化 目标 为 该 误差 的 二 范 数 ， 暂 时 

取 不 加 权 的 形式 ， 为 


min J (€) = llel’. (8.11) 
能 够 做 这 种 优化 的 理由 ， 仍 是 基于 灰 度 不 变 假设 。 在 直接 法 中 ， 我 


们 假设 一 个 空间 点 在 各 个 视角 下 成 像 的 灰 度 是 不 变 的。 我 们 有 许多 个 
(比如 N 个 ) 空间 点 P; ， 那 么 ， 整 个 相机 位 资 估 计 问 题 变 六 


mind (£) = X efei， ei = l (p11) — Te (poi). (8.12) 


注意 这 里 的 优化 变量 是 相机 位 次 5 。 为 了 求解 这 个 优化 问题 ， 我 们 关 
心 误差 e 是 如 何 随 着 相机 位 姿 5 变 化 的 ， 需 要 分 析 它 们 的 导数 天 系 。 
此 ， 使 用 李 代 数 上 的 扰动 模型 。 我 们 给 exp(é ) 左 乘 一 个 小 扰动 exp(65 )， 


fz. © 


1 


7K exp (86^ exp (E^) P) 


e 花 @56 = (zxr) = ( 
=h (3P) Li Gas (1 + SE^) exp (€") P) 
=f (FKP) ie (35e (^P + 7 Kee exp (£^) P) 
类 似 于 上 一 讲 ， 记 : 
q = 6é° exp (Ẹ^) P, 
TEE 


Z2 


这 里 的 q 为 扰动 分 量 在 第 二 个 相机 坐标 系 下 的 坐标 ， 而 u 为 它 的 像素 
坐标 。 利用 一 阶 泰 勒 展开 ， 有 : 


1 


e(€ 068) = ( KP) =P (Kop @)P+u) 


Z1 
7 zl E 下 X OIn ðu ðq 
~I, (KP) (Kept )P) nA 
_ ðI Ou ðq 
= @(8) — Su aq Dees 


我 们 看 到 ， 一 阶 导数 由 于 链 式 法 则 分 成 了 3 项 ， 而 这 3 项 都 是 容易 计 
算 的 : 


1.01 , /6u Au 处 的 像素 梯度 。 


2.0u /09 为 投影 方程 天 于 相机 坐标 系 下 的 三 维 点 的 导数 。 记 q X,Y,Z 
]7 ， 根 据 上 一 节 的 推导 ， 导 数 为 


(a Ou Ou fe fae X 

ðu | ox ay az |_| z 0 一 克 
一 一 (8.13) 

ðq Ov Ov Ov 0 fy _faY 

Ox OY O24 Z Z2 


3.0q /665 为 变换 后 的 三 维 点 对 变换 的 导数 ， 这 在 李 代 数 一 讲 介绍 过 
T: 


o 
pg a. (8.14) 
在 实践 中 ， 由 于 后 两 项 只 与 三 维 点 qd 有关， 而 与 图 像 无 和 天， 我 们 经 党 
把 它 合 并 在 一 起 : 
ðu _ | & 0 -4E -X 7,445 -学 
aë z f. fyY fuY? fy XY fX (8.15) 
0 y ve f. y 5 y 5 y 
Z Z y Z Z Z 


这 个 2x 6 的 矩阵 在 上 一 讲 中 也 出 现 过 。 于 是 ， 我 们 推导 出 误差 相对 
于 李 代 数 的 雅 可 比 矩 阵 : 
OL» Ou 


对 于 NN 个 点 的 问题 ， 我 们 可 以 用 这 种 方法 计算 优化 问题 的 雅 可 比 矩 
阵 ， 然 后 使 用 高 斯 牛顿 法 或 列 文 伯 格 一 马 夸 尔 特 方法 计算 增 量 ， 迭 代 求 
解 。 至 此 ， 我 们 推导 了 直接 法 估计 相机 位 资 的 整个 流程 ， 下 面 通过 程序 
来 演示 一 下 直接 法 是 如 何 使 用 的 。 


8.4.2 ”直接 法 的 讨论 
在 上 面 的 推导 中 ，P 是 一 个 已 知 位 置 的 空间 点 ， 它 是 怎么 来 的 呢 ? 在 


RGB-D 相 机 下 ， 我 们 可 以 把 任意 像素 反 投 影 到 三 维 空间 ， 然 后 投影 到 下 
一 幅 图 像 中 。 如 果 在 单 目 相机 中 ， 这 件 事情 要 更 为 困难 ， 因 为 我 们 还 须 


考虑 由 P 的 深度 带 来 的 不 确定 性 。 详 细 的 深度 估计 放 到 第 13 讲 中 讨论 。 现 
在 我 们 先 来 考虑 简单 的 情况 ， 即 P 深度 已 知 的 情况 。 

根据 P 的 来 源 ， 我 们 可 以 把 直接 法 进行 分 类 : 

1.P 来 目 于 稀 玻 关键 点 ， 我 们 称 之 为 稀 臣 直接 法 。 通 党 我 们 使 用 数 自 
个 至 上 于 个 关键 吕 ， 并 且 像 L-K 光 流 那样 ， 假 设 它 周 围 像素 也 是 不 变 的 。 
这 种 稀 忠 直接 法 不 必 计 算 摘 述 子 ， 并 且 只 使 用 数 特 个 像素 ， 因 此 速度 最 
快 ， 但 只 能 计算 稀 朴 的 重 构 。 


2P 来 自 部 分 像素 。 我 们 看 到 式 (8.16) 中 ， 如 果 像 素 梯度 为 零 ， 整 项 
雅 可 比 和 矩阵 就 为 零 ， 不 会 对 计算 运动 增 量 有 任何 贡献 。 因 此 ， 可 以 考虑 
只 使 用 带 有 梯度 的 像素 点 ， 舍 弃 像 素 梯度 不 明显 的 地 方 。 这 称 为 半 稠 密 
(Semi-Dense) 的 直接 法 ， 可 以 重 构 一 个 半 稠 密 结构 。 


3.P 为 所 有 像素 ， 称 为 稠密 直接 法 。 稠 密 重 构 需要 计算 所 有 像素 (一 
般 几 十 万 至 几 百 万 个 ) ， 因 此 多 数 不 能 在 现 有 的 CPU 上 实时 计算 ， 需 要 
GPU 的 加 速 。 但 是 ， 如 前 面 所 讨论 的 ， 像 素 梯度 不 明显 的 点 ， 在 运动 估 
计 中 不 会 有 太 大 贡献 ， 在 重 构 时 也 会 难以 估计 位 置 。 


可 以 看 到 ， 从 稀 足 到 稠密 重 构 ， 都 可 以 用 直接 法 来 计算 。 它 们 的 计 
SSSA KN. MRA AD ARR KAN UA, MBA) 
以 建立 完整 地 图 。 具 体 使 用 哪 种 方法 ， 需 要 视 机 器 人 的 应 用 环境 而 定 。 
特别 地 ， 在 低 端的 计算 平台 上 ， 稀 跑 直接 法 可 以 做 到 非常 快速 的 效果 ， 
适用 于 实时 性 较 高 且 计 算 资源 有 限 的 场合 51。 


8.5 ”实践 RGB-D 的 直接 法 
8.5.1 ”稀疏 直接 法 


现在 ， 我 们 来 演示 如 何 使 用 稀 下 的 直接 法 。 由 于 本 书 不 涉及 GPU 编 
程 ， 稠 密 的 直接 法 就 省 略 掉 了 。 同 时 ， 为 了 保持 程序 简单 ， 我 们 使 用 
RGB-D 数 据 而 非 单 目 数据 ， 这 样 可 以 省 略 掉 单 目的 深 度 恢复 部 分 。 基 于 
特征 点 的 深度 恢复 已 经 在 上 一 讲 介 绍 过 ， 而 基于 块 匹 配 的 深度 恢复 将 在 
后 面 介绍 。 所 以 本 世 我 们 来 考虑 RGB-D 上 的 稀 中 直接 法 VO。 


由 于 求解 直接 法 最 后 等 价 于 求解 一 个 优化 问题 ， 因 此 可 以 使 用 g2o 或 
Ceres 这 些 优化 库 来 帮助 求解 。 本 节 以 g2o 为 例 设 计 实 验 ， 而 Ceres 部 分 则 
留 作 习题 。 在 使 用 g2o 之 前 ， 需 要 把 直接 法 抽象 成 一 个 图 优化 问题 。 显 
然 ， 直 接 法 是 由 以 下 顶点 和 边 组 成 的 : 


1. 优 化 变量 为 一 个 相机 位 姿 ， 因 此 需要 一 个 位 姿 顶 点 。 由 于 我 们 在 推 
导 中 使 用 了 李 代 数 ， 故 程序 中 使 用 李 代 数 表达 的 SE(3) 位 姿 顶 点 。 与 上 一 
讲 一 样 ， 我 们 将 使 用 “VertexSE3Expmap” 作 为 相机 位 姿 。 

2. 误 差 项 为 单个 像素 的 光度 误差 。 由 于 整个 优化 过 程 中 1, (p ; ) 保 持 不 
变 ， 我 们 可 以 把 它 当 成 一 个 固定 的 预 设 值 ， 然 后 调整 相机 位 姿 ， 使 1 , (p ， 
) 接 近 这 个 值 。 于 是 ， 这 种 边 只 连接 一 个 顶点 ， 为 一 元 边 。 由 于 g2o 中 本 
身 没 有 计算 光度 误差 的 边 ， 我 们 需要 自己 定义 一 种 新 的 边 。 

在 上 述 的 建 模 中 ， 直 接 法 图 优化 问题 是 由 一 个 相机 位 姿 顶 点 与 许多 
条 一 元 边 组 成 的 。 如 果 使 用 稀 疏 的 直接 法 ， 那 我 们 大 约会 有 几 百 至 几 千 
条 这 样 的 边 ; 稠密 直接 法 则 会 有 几 十 万 条 边 。 优 化 问题 对 应 的 线性 方程 
是 计算 李 代 数 增 量 ， 本 身 规模 不 大 (6x6) ， 所 以 主要 的 计算 时 间 会 花 
费 在 每 条 边 的 误差 与 雅 可 比 和 矩阵 的 计算 上 。 下 面 的 实验 中 ， 我 们 先 来 定 
义 一 种 用 于 直接 法 位 资 估 计 的 边 ， 然 后 ， 使 用 该 边 构建 图 优化 问题 并 求 


解 。 
8.5.2 ”定义 直接 法 的 边 
首先 定义 计算 光度 误差 的 边 。 按 照 前 面 的 推导 ， 还 需要 给 出 它 的 牙 


可 比 和 矩阵 : 
内 slambook/ch8/directMethod/direct_sparse.cpp (片段 ) 


// project a 3d point into an image plane, the error is photometric error 


// an unary edge with one vertex SE3Expmap (the pose of camera) 
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap> 
{ 
public: 
EIGEN_MAKE_ALIGNED_OPERATOR_NEW 


EdgeSE3ProjectDirect() {} 

EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, 
float cy, cv::Mat* image ) : x_world_ ( point ), fx ( fx ), fy- ( fy Js wr ( 
cx ), cy_ ( cy ), image_ ( image ) 


{} 


virtual void computeError() 


{ 
const VertexSE3Expmap* v =static_cast<const VertexSE3Expmap*> ( _vertices 
[0] ); 
Eigen: :Vector3d x_local = v->estimate().map ( x_world_ ); 
float x = x_local[0]*fx_/x_local[2] + cx_; 
float y = x_local[1]*fy_/x_local[2] + cy_; 
// check x,y is in the image 
if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows 
) 
{ 
_error ( 0,0 ) = 0.0; 
this->setLevel ( 1 ); 
} 
else 
{ 
error ( 0,0 ) = getPixelValue ( x,y ) - _measurement; 
} 
} 


// plus in manifold 
virtual void linearize0plus( ) 
{ 

if ( level() == 1 ) 

{ 


_jacobianOplusXi = Eigen: :Matrix<double, 1, 6>::Zero(); 


37 


38 


71 


return; 


} 


VertexSE3Expmap* vtx = 


stat 


Eigen: :Vector3d xyz_trans = 


book 

double x = xyz_trans [0]; 
double y = xyz_trans[1]; 
double invz = 1.0/xyz_trans 
double invz_2 = invz*invz; 
float u = x*fx_*invz + cx_; 
float v = y*fy_*invz + cy_; 


// jacobian from se3 to u,v 


// NOTE that in 92o the Lie algebra is (\omega, \epsilon), where \omega is 


ic_cast<VertexSE3Expmap*> ( _vertices[0] ); 


vtx->estimate().map ( x_world_ ); 


[2]; 


so(3) and \epsilon the translation 


Eigen: :Matrix<double, 2, 


jacobian_uv_ksai 
jacobian_uv_ksai 
jacobian_uv_ksai 
jacobian_uv_ksai 
jacobian_uv_ksai 


jacobian_uv_ksai 


jacobian_uv_ksai 
jacobian_uv_ksai 
jacobian_uv_ksai 
jacobian_uv_ksai 
jacobian_uv_ksai 


jacobian_uv_ksai 


Eigen: :Matrix<double, 1, 


jacobian_pixel_uv ( 0,0 ) 


6> 


0,0 
G1 
0,2 
0,3 
0,4 
0,5 


天 到 天 天 天 天 
v v v v Ţv YL 
il ll 


作 个 个 个 个 个 
YY v wv vw 


2> 


) /2; 

jacobian_pixel_uv ( 0,1 ) = 
) > /2; 

_jacobianOplusXi = jacobian 


// dummy read and write functions because we don't care... 


jacobian_uv_ksai; 


- x*y*invz_2 *fx_; 


( 1+ ( x*x*invz_2 ) ) *fx_; 


- y*invz *fx_; 
invz *fx_; 
0; 


-x*invz_2 *fx_; 


- ( ity*y*invz_2 ) *fy_; 


x*y*invz_2 *fy_; 
x*invz *fy_; 

0; 

inVZ *fy_; 


-y*invz_2 *fy_; 


jacobian_pixel_uv; 


( getPixelValue ( u+1,v )-getPixelValue ( u-1,v 


( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 


_pixel_uv*jacobian_uv_ksai; 


virtual bool read ( std::istream& in ) {} 


78 virtual bool write ( std::ostream& out ) const {} 

79 

80 | protected: 

81 // get a gray scale value from reference image (bilinear interpolated) 
82 inline float getPixelValue ( float x, float y ) 

83 { 

84 uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ]; 
85 float xx = x - floor ( x ); 

86 float yy = y - floor ( y ); 

87 return float ( 

88 ( 1-xx ) * ( 1-yy ) * data[0] + 

89 xx* ( 1-yy ) * data[i] + 

90 ( 1-xx ) *yy*data[ image_->step ] + 

91 xx*yy*data[image_->step+1] 

92 ) 

93 } 

94 | public: 

95 Eigen: :Vector3d x_world_; // 3D point in world frame 
96 float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics 
97 cv::Mat* image_=nullptr; // reference image 

ə |}; 


我 们 的 边 继承 自 g2o::BaseUnaryEdge。 在 继承 时 ， 需 要 在 模板 参数 里 

填 入 测量 值 的 维度 、 类 型 ， 以 及 连接 此 边 的 顶点 ， 同 时 ， 我 们 把 空间 点 P 
、 相 机 内 参 和 图 像 存储 在 该 边 的 成 员 变 量 中 。 为 了 让 g2o 优 化 该 边 对 应 的 
误差 ， 我 们 需要 履 写 两 个 虚 辆 数 : 用 computeErrorO 计算 误差 值 ， 用 
linearizeOplus() 计 算 雅 可 比 和 矩阵 。 可 以 看 到 ， 这 里 的 雅 可 比 和 矩阵 计算 与 式 
(8.16) 是 一 致 的 。 注 意 我 们 在 程序 中 的 误差 计算 里 使 用 了 I,(p,)-I,(p,) 


的 形式 ， 因 此 前 面 的 负 号 可 以 省 去 ， 只 需 把 像素 梯度 乘 以 像素 到 李 代 数 
的 梯度 即 可 。 


在 程序 中 ， 相 机 位 姿 是 用 浮 点 数 表示 的 ， 投 影 到 像素 坐标 也 是 浮 点 
形式 。 为 了 更 精细 地 计算 像素 亮度 ， 我 们 要 对 图 像 进 行 插 值 。 我 们 这 里 
采用 了 简单 的 双 线 性 插值 ， 也 可 以 使 用 更 复杂 的 插值 方式 ， 但 计算 代价 


ATK 
可 能 会 变 高 一 些 。 


8.5.3 ”使 用 直接 法 估计 相机 运动 


定义 了 g2o 边 后 ， 我 们 将 节点 和 边 组 合成 图 ， 就 可 以 调用 g2o 进 行 优 
化 了 。 实 现代 码 位 于 slambook/ch8/directMethod/direct_sparse.cpp 中 ， 请 读 
者 阅读 该 部 分 代码 并 编译 。 


在 这 个 实验 中 ， 我 们 读 取 数 据 集 的 RGB-D 图 像 序列 。 以 第 一 幅 图 像 
为 参考 帧 ， 然 后 用 直接 法 求解 后 续 图 像 的 位 资 。 在 参考 帧 中 ， 对 第 一 幅 
图 像 提取 FAST 关 键 点 〈 不 需要 描述 子 ) ， 并 使 用 直接 法 估计 这 些 关 键 点 
在 第 二 幅 图 像 中 的 位 置 ， 以 及 第 二 幅 图 像 的 相机 位 姿 。 这 就 构成 了 一 种 
简单 的 稀 牙 直接 法 。 最 后 ， 我 们 画 出 这 些 天 键 点 在 第 二 幅 图 像 中 的 投 


执行 如 下 命令 : 


1 


build/direct_sparse ~/dataset/rgbd_dataset_freiburg1i_desk | 


程序 会 在 作 图 之 后 暂停 ， 你 可 以 看 到 特征 点 的 位 置 关 系 ， 终 端 也 会 
输出 迭代 误差 的 下 降 过 程 。 

如 图 8-4 所 示 ， 我 们 看 到 在 两 幅 图 像 相 差不多 时 ， 直 接 法 会 调整 相机 
的 位 姿 ， 使 得 大 部 分 像素 都 能 够 正确 跟踪 。 但 是 ， 在 稍 长 一 点 的 时 间 
内 ， 比 如 说 0~9 帧 之 间 的 对 比 ， 我 们 发 现 由 于 相机 位 次 估计 不 准确 ， 特 
征 点 出 现 了 明显 的 偏 移 现象 。 我 们 会 在 本 讲 末 尾 对 其 进行 分 析 。 


0-1 0-3 0-9 


图 8-4 ”稀疏 直接 法 的 实验 。 左 : 误差 随 着 迭代 下 降 。 右 : BAWAL GERA 
DRIER 


i) o 


8.5.4” 半 稠密 直接 法 


我 们 很 容易 就 能 把 程序 拓展 成 半 稠 密 的 直接 法 形式 。 对 参考 帧 ， 先 
提取 梯度 较 明 显 的 像素 ， 然 后 用 直接 法 ， 以 这 些 像素 为 图 优化 边 来 估计 
相机 运动 。 对 先前 的 程序 做 如 下 修改 : 


内 slambook/ch8/direct_semidense.cpp 


1 |// select the pizels with high gradiants 

2 |for ( int x=10; x<gray.cols-10; x++ ) 

3 for ( int y=10; y<gray.rows-10; y++ ) 

4 { 

5 Eigen::Vector2d delta ( 

6 gray.ptr<uchar>(y) [x+1] - gray.ptr<uchar>(y) [x-1], 

7 gray.ptr<uchar>(y+1) [x] - gray.ptr<uchar>(y-1) [x] 

8 J; 

9 if ( delta.norm() < 50 ) 

10 continue; 

11 ushort d = depth.ptr<ushort> (y) [x]; 

12 if ( d==0 ) 

13 continue; 

14 Eigen: :Vector3d p3d = project2Dto3D ( x, y, d, fx, fy, cx, cy, depth_scale 
); 

15 float grayscale = float ( gray.ptr<uchar> (y) [x] ); 

16 measurements .push_back ( Measurement ( p3d, grayscale ) ); 

17 } 
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显 梯度 的 像素 。 于 是 在 图 优化 中 会 增加 许多 的 边 。 这 些 边 都 会 参与 估计 
相机 位 次 的 优化 间 题 ， 利 用 大 量 的 像素 而 不 单单 是 稀 艳 的 特征 点 。 由 于 
我 们 并 没有 使 用 所 有 的 像素 ， 所 以 这 种 方式 又 称 为 半 稠 密 方法 ( Semi- 
dense) 。 我 们 把 参与 估计 的 像素 取出 来 并 在 图 像 中 显示 出 来 ， 如 图 8-5 所 
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图 8-5“ 半 稠密 直接 法 的 实验 。 参 考 帧 与 第 2,5,8 帧 的 对 比 ， 绿 色 部 分 标 出 了 参与 优化 的 像 
素 。 


如 果 读 者 亲自 做 了 实验 ， 就 可 以 看 到 参与 估计 的 像素 像 是 固定 在 空 
间 中 一 样 。 当 相机 旋转 时 ， 它 们 的 位 置 似乎 没有 发 生变 化 。 这 代表 了 我 
们 估计 的 相机 运动 是 正确 的 。 同 时 ， 你 可 以 检查 使 用 的 像素 数量 与 优化 
时 间 的 关系 。 显 然 ， 当 像素 增多 时 ， 优 化 会 更 加 费时 ， 所 以 为 了 实时 
性 ， 需 要 考虑 使 用 较 好 的 像素 点 ， 或 者 降低 图 像 的 分 辩 率 。 不 过 对 于 演 
示 实 验 来 说 ， 笔 者 认为 这 样 已 经 能 够 理解 直接 法 的 意义 了 。 


8.5.5 ”直接 法 的 讨论 


相 比 于 特征 点 法 ， 直 接 法 完全 依靠 优化 来 求解 相机 位 姿 。 从 式 (8.16) 
中 可 以 看 到 ， 像 素 梯度 引导 着 优化 的 方向 。 如 果 想 要 得 到 正确 的 优化 结 
果 ， 就 必须 保证 大 部 分 像素 梯度 能 够 把 优化 引导 到 正确 的 方向 。 


这 是 什么 意思 呢 ? 我 们 不 妨 设身处地 地 扮演 一 下 优化 算法 。 假 设 对 
于 参考 图 像 ， 我 们 测量 到 一 个 灰 度 值 为 229 的 像素 。 并 且 ， 由 于 我 们 知道 
它 的 深度 ， 可 以 推断 出 空间 点 P 的 位 置 (图 8-6 所 示 在 1, 中 测量 到 的 灰 
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图 8-6 一 次 迭代 的 图 形 化 显示 。 


此 时 我 们 又 得 到 了 一 幅 新 的 图 像 ， 需 要 估计 它 的 相机 位 姿 。 这 个 位 
姿 是 由 一 个 初 值 不 断 地 优化 迭代 得 到 的 。 假 设 我 们 的 初 值 比较 差 ， 在 这 
个 初 值 下 ， 空 间 点 P 投影 后 的 像素 灰 度 值 是 126。 于 是 ， 这 个 像素 的 误差 
为 229- 126=103。 为 了 减 小 这 个 误差 ， 我 们 希望 微调 相机 的 位 姿 ， 使 像素 
更 亮 一 些 。 

怎么 知道 往 哪里 微调 像素 会 更 亮 呢 ? 这 就 需要 用 到 局 部 的 像素 梯 
度 。 我 们 在 图 像 中 发 现 ， 沿 u 轴 往 前 走 一 步 ， 该 处 的 灰 度 值 变 成 了 123， 
即 减 去 了 3。 同 样 地 ， 沿 v 轴 往 前 走 一 步 ， 灰 度 值 减 了 18， 变 成 108。 在 这 
个 像素 周围 ， 我 们 看 到 梯度 是 [- 3,- 18]， 为 了 提高 亮度 ， 我 们 会 建议 优化 
算法 微调 相机 ， 使 P 的 像 往 左上 方 移动。 在 这 个 过 程 中 ， 我 们 用 像素 的 
局 部 梯度 近似 了 它 附近 的 灰 度 分 布 ， 不 过 请 注意 ， 真 实 图 像 并 不 是 光滑 
的 ， 所 以 这 个 梯度 在 远 处 就 不 成 立 了 。 

但 是 ， 优 化 算法 不 能 只 听 这 个 像素 的 一 面 之 词 ， 还 需要 听取 其 他 像 
素 的 建议 中 。 综 合 听 取 了 许多 像素 的 意见 之 后 ， 优 化 算法 选择 了 一 个 和 
我 们 建议 的 方向 偏离 不 远 的 地 方 ， 计 算出 一 个 更 新 量 exp(E^ )。 加 上 更 新 
量 后 ， 图 像 从 I ,移动 到 了 ， 像 素 的 投影 位 置 也 变 到 了 一 个 更 亮 的 地 
方 。 我 们 看 到 ， 通 过 这 次 更 新 ， 误 差 变 小 了 。 在 理想 情况 下 ， 我 们 期 望 
误差 会 不 断 下 降 ， 最 后 收敛 。 


但 是 实际 是 不 是 这 样 呢 ?我 们 是 否 真 的 只 要 治 着 梯度 方向 走 ， 就 能 
走 到 一 个 最 优 值 ? 注意 到 ， 直 接 法 的 梯度 是 直接 由 图 像 梯度 确定 的 ， 因 
此 我 们 必须 保证 沿 着 图 像 梯度 走时 ， 灰 度 误 差 会 不 断 下 降 。 然 而 ， 图 像 
通常 是 一 个 很 强烈 的 非 凸 立 数 ， 如 图 8-7 所 示 。 实 际 当 中 ， 如 果 我 们 沿 着 
图 像 梯 度 前 进 ， 很 容易 由 于 图 像 本 身 的 非 凸 性 《或 噪声 ) 落 进 一 个 局 部 
极 小 值 中 ， 无 法 继续 优化 。 只 有 当 相 机 运动 很 小 ， 图 像 中 的 梯度 不 会 有 
很 强 的 非 凸 性 时 ， 直 接 法 才能 成 立 。 


图 8-7 一 张 图 像 的 三 维 化 显示 。 从 图 像 中 的 一 个 点 运动 到 另 一 个 点 的 路 径 不 见得 是 “笔直 的 
下 坡 路 "， 而 需要 经 常 < 翻 山越 岭 "。 这 体现 了 图 像 本 身 的 非 凸 性 。 


在 例 程 中 ， 我 们 只 计算 了 单个 像素 的 差异 ， 并 且 这 个 差异 是 由 灰 度 
直接 相 减 得 到 的 。 然 而 ， 单 个 像素 没有 什么 区 分 性 ， 周 围 很 可 能 有 好 多 
像素 和 它 的 亮度 差不多 。 所 以 ,我们 有 时 会 使 用 小 的 图 像 块 (patch) , 
并 且 使 用 更 复杂 的 差异 度量 方式 ， 例 如 归 一 化 相关 性 (Normalized Cross 
Correlation, NCC) 等 〈 见 第 13 讲 ) 。 而 例 程 为 了 简单 起 见 ， 使 用 了 误差 
的 平方 和 ， 以 保持 与 推导 的 一 致 性 。 


85.6 直接 法 优 缺 点 总 结 


最 后 ， 我 们 总 结 一 下 直接 法 的 优 缺 点 。 大 体 上 ， 它 的 优点 如 下 : 
“可 以 省 去 计算 特征 点 、 摘 述 子 的 时 间 。 


“只 要 求 有 像素 梯度 即 可 ， 不 需要 特征 点 。 因 此 ， 直 接 法 可 以 在 特征 
缺失 的 场合 下 使 用 。 比 较 极端 的 例子 是 只 有 渐变 的 一 幅 图 像 。 它 可 能 
法 提取 角 点 类 特征 ， 但 可 以 用 直接 法 估计 它 的 运动 。 


“可 以 构建 半 稠 密 旋 至 稠密 的 地 图 ， 这 是 特征 点 法 无 法 做 到 的 。 
另 一 方面 ， 它 的 缺点 也 很 明显 : 


. 非 凸 性 。 直 接 法 完全 依靠 梯度 搜索 ， 降 低 目 标 函 数 来 计算 相机 位 
姿 。 其 目标 函数 中 需要 取 像素 点 的 灰 度 值 ， 而 图 像 是 强烈 非 凸 的 函数 。 
这 使 得 优化 算法 容易 进入 极 小 ， 只 在 运动 很 小 时 直接 法 才能 成 功 。 

.单个 像素 没有 区 分 度 。 和 它 像 的 实在 太 多 了 ! 于 是 我 们 要 么 计算 图 
像 块 ， 要 么 计算 复杂 的 相关 性 。 由 于 每 个 像素 对 改变 相机 运动 的 "意见 ” 
不 一 致 ， 只 能 少数 服从 多 数 ， 以 数量 代替 质量 。 

. 灰 度 值 不 变 是 很 强 的 假设 。 如 果 相 机 是 自动 曝光 的 ， 当 它 调整 曝光 
参数 时 ， 会 使 得 图 像 整 体 变 亮 或 变 暗 。 光 照 变化 时 亦 会 出 现 这 种 情况 。 
特征 点 法 对 光照 具有 一 定 的 容忍 性 ， 而 直接 法 由 于 计算 灰 度 间 的 差异 ， 
整体 灰 度 变化 会 破坏 灰 度 不 变 假设 ， 使 算法 失败 。 针 对 这 一 点 ， 目 前 的 
直接 法 开始 使 用 更 细致 的 光度 模型 标定 相机 ， 以 便 在 曝光 时 间 变化 时 也 
能 工作 。 

习题 

1 除了 LK 光 流 之 外 ， 还 有 哪些 光 流 方法 ? 它们 各 有 什么 特点 ? 

2. 在 本 节 程 序 的 求 图 像 梯度 过 程 中 ， 我 们 简单 地 求 了 u +1 和 u- 1 的 灰 
度 之 差 除 以 7， 作 为 u 方向 上 的 梯度 值 。 这 种 做 法 有 什么 缺点 ? 提示 : 对 
于 距离 较 近 的 特征 ， 变 化 应 该 较 快 ; 而 距离 较 远 的 特征 在 图 像 中 变化 较 
慢 ， 求 梯度 时 能 否 利用 此 信息 ? 

3. 在 稀疏 直接 法 中 ， 假 设 单个 像素 周围 小 块 的 光度 也 不 变 ， 是 否 可 以 
提高 算法 健壮 性 ”请 编程 实现 。 

4.* 使 用 Ceres 实 现 RGB-D 上 的 稀疏 直接 法 和 半 稠 密 直接 法 。 

5. 相 比 于 RGB-D 的 直接 法 ， 单 目 直接 法 往往 更 加 复杂 。 除 了 匹配 未 知 
之 外 ， 像 素 的 距离 也 是 待 估计 的 ， 我 们 需要 在 优化 时 把 像素 深度 也 作为 
优化 变量 。 阅 读 文 献 [59,57]， 你 能 理解 它 的 原理 吗 ? 如 果 不 能 ， 请 在 第 
13 讲 后 再 回来 阅读 。 


6. 由 于 图 像 的 非 吓 性 ， 直 接 法 目前 还 只 能 用 于 短 距 离 、 非 自动 曝光 的 
相机 。 你 能 否 提出 增强 直接 法 健壮 性 的 方案 ”阅读 文献 [58,60] 可 能 会 给 


你 一 些 灵 感 。 


[1] 见 http:/vision.in.tum.de/data/datasets/rgbd-dataset/download。 


[2] 为 了 避免 齐 次 / 非 齐 次 坐标 转换 而 导致 的 公式 形式 复杂 化 ， 我 们 假设 中 间 隐 式 地 做 了 所 需 的 变 
化 。 它 不 会 影响 公式 的 推导 。 


[3] 这 可 能 是 一 种 不 严谨 的 拟人 化 说 法 ， 不 过 有 助 于 理解 。 


第 9 讲 ”实践 : 设计 前 端 


主要 目标 

1. 实 际 设 计 一 个 视觉 里 程 计 前 端 。 

2. 理 解 SLAM 软 件 框 架 是 如 何 搭建 的 。 

3. 理 解 在 前 端 设计 中 容易 出 现 的 问题 ， 以 及 修补 的 方式 。 

本 讲 是 全 书 比 较 少 见 的 完全 由 实践 部 分 组 成 的 一 讲 。 我 们 将 使 用 前 
两 讲学 到 的 知识 ， 实 际 书写 一 个 视觉 里 程 计 程序 。 你 会 管理 局 部 的 机 器 
人 轨迹 与 路 标点 ， 并 体验 一 下 一 个 软件 框架 是 如 何 组 成 的 。 在 操作 过 程 
中 ， 我 们 会 遇 到 许多 问题 : 相机 运动 过 快 、 图 像 模 糊 、 误 匹配 ...... 都 会 
使 算法 失效 。 要 让 程序 稳定 运行 ， 我 们 需要 处 理 以 上 的 种 种 情况 ， 这 将 
带 来 许多 工程 实现 方面 的 、 有 益 的 讨论 。 


9.1 措 建 VO 框 染 


知晓 砖头 和 水 泥 的 原理 ， 并 不 代表 能 够 建造 伟大 的 宫殿 。 


在 笔者 深 爱 的 《我 的 世界 》 游 戏 中 ， 玩 家 拥有 的 只 是 一 些 色彩 、 纹 
理 不 同 的 方块 。 其 性 质 极其 简单 ， 而 玩家 所 要 做 的 只 是 把 这 些 方块 放 在 
空地 上 而 已 。 理 解 一 个 方块 至 为 简单 ， 但 实际 拿 起 它们 时 ， 初 学 者 往往 
只 能 搭建 简单 的 火 案 使 房 屋 ， 而 有 经 验 、 有 创造 力 的 玩家 则 可 用 这 些 简 
单 的 方块 建造 民居 、 园 林 、 楼 台 襄 榭 ,乃至 城市 (图 9-1) "。 


图 9-1 从 简单 的 事物 出 发 ， 逐 渐 搭 建 越 来 越 复 杂 但 越 来 越 优秀 的 作品 。 


在 SLAM 中 ， 我 们 认为 工程 实现 和 理解 算法 原理 应 该 至 少 是 同等 重要 
的 ， 甚 至 更 应 强调 如 何 书写 实际 可 用 的 程序 。 算 法 的 原理 ， 融 像 一 个 个 
方块 一 样 ， 我 们 可 以 清楚 明确 地 讨论 它们 的 原理 和 性 质 ， 但 仅仅 理解 了 
一 个 个 方块 并 不 能 使 你 建造 真正 的 建筑 : 它们 需要 大 量 的 尝试 、 时 间 和 
经 验 ， 我 们 鼓励 读者 朝 更 为 实际 的 方向 努力 一 一 当然 这 往往 是 十 分 复杂 
的 。 就 像 在 《我 的 世界 》 里 那样 ， 你 需要 掌握 各 种 立柱 、 墙 面 、 屋 项 的 
结构 ， 墙 面 的 雕花 ， 几 何 形体 角度 的 计算 ， 这 些 远 远 不 像 讨论 每 个 方块 
的 性 质 那样 简单 。 


SLAM 的 具体 实现 亦 是 如 此 ， 一 个 实用 的 程序 会 有 很 多 的 工程 设计 和 
技巧 (Trick) ， 还 需要 讨论 每 一 步 出 现 问题 之 后 该 如 何 处 理 。 原 则 上 
讲 ， 每 个 人 实现 的 SLAM 都 会 有 所 不 同 ， 多 数 时 候 我 们 并 不 能 说 哪 种 实现 
方式 就 一 定 是 最 好 的 。 但 是 ， 我 们 通常 会 遇 到 一 些 共同 的 问题 “怎么 管 
理 地 图 点 “如 何 处 理 误 匹 配 ”“ 如 何 选 择 关键 帧 "， 等 寺 。 我 们 希望 读者 能 
对 这 些 可 能 出 现 的 问题 产生 一 些 直 观 的 感觉 一 一 我 们 认为 这 种 感觉 是 非 
单 重 要 的 。 


所 以 ， 出 于 对 实践 的 重视 ， 本 章 我 们 将 带领 读者 领略 一 下 搭建 SLAM 
框架 的 过 程 。 就 像 建 筑 那 样 ， 我 们 要 讨论 柱 间 距 、 门 面 宽 高 比 等 琐碎 但 
重要 的 问题 。SLAM 工 程 是 复杂 的 。 即 使 我 们 只 保留 核心 的 部 分 ， 也 会 占 
用 大 量 的 篇 幅 ， 使 本 书 变 得 过 于 繁 几 。 不 过 ， 请 注意 ， 尽 管 完成 之 后 的 
工程 是 复杂 的 ， 但 是 中 间 的 “由 简 到 繁 ”的 过 程 ， 是 值得 详细 讨论 、 有 学 
习 价 值 的 。 所 以 ， 我 们 要 从 简单 的 数据 结构 出 发 ， 先 来 做 一 个 简单 的 视 


觉 里 程 计 ， 再 慢 慢 地 把 一 些 额 外 的 功能 加 进来 。 换 言 之 ， 我 们 要 把 从 简 
单 到 复杂 的 过 程 展现 给 读者 看 ， 这 样 你 才 会 明白 一 个 库 是 如 何 像 雪 人 和 那 
样 慢 慢 堆 起 来 的 。 

本 讲 的 代码 放 在 slambook/project 中 。 由 于 随 着 开发 过 程 不 断 前 进 ， 
我 们 会 对 工程 做 一 些 删 改 ， 因 此 它 的 内 容 也 会 发 生变 化 。 所 以 我 们 会 把 
中 间 的 代码 也 保留 在 目录 中 ， 以 版 本 号 命名 ， 以 便 读 者 随时 查看 、 模 
仿 。 


9.1.1 确定 程序 框 染 


根据 前 两 讲 的 内 容 ， 我 们 知道 视觉 里 程 计 分 单 目 、 双 目 、RGB-D 三 
大 类 。 单 目 视 觉 相 对 复杂 ， 而 RGB-D 最 为 简单 ， 没 有 初始 化 ， 也 没有 尺 
度 问 题 。 本 着 由 简 入 繁 的 指导 思想 ， 我 们 先 从 RGB-D 做 起 。 为 了 方便 读 
者 做 实验 ， 我 们 将 使 用 数据 集 而 非 实 际 的 RGB-D 相 机 (因为 不 能 保证 读 
者 人 手 一 台 RGB-D 相 机 ) 。 


首先 ， 我 们 来 了 解 一 下 Linux 程 序 的 组 织 方式 。 在 编写 一 个 小 规模 的 
库 时 ， 我 们 通常 会 建立 一 些 文件 来， 把 源 代码 、 头 文件 、 文 档 、 测 试 数 
据 、 配 置 文件 、 日 志 等 分 类 存放 ， 这 样 会 显得 很 有 条 理 。 如 果 一 个 库 内 
容 很 多 ， 我 们 还 会 把 代码 分 解 成 各 个 独立 的 小 模块 ， 以 便 测试 。 读 者 可 
以 参照 OpenCV 或 g2o 的 组 织 方 式 ， 看 看 一 个 大 中 型 库 是 如 何 组 织 的 。 例 
如 ，OpenCV 有 core、imgproc、features2d 等 模块 ， 每 个 模块 分 别 负责 不 同 
的 任务 。g20 则 有 core、solvers、types 等 若干 模块 。 不 过 在 小 型 程序 里 ， 
我 们 也 可 以 把 所 有 的 东西 友 在 一 起 ， 称 为 SLAM 库 。 


现在 我 们 要 写 的 SLAM 库 是 一 个 小 型 库 ， 目 标 是 帮 读 者 将 本 书 用 到 的 
各 种 算法 融会 贯通 ， 书 写 自 己 的 SLAM 程 序 。 挑 选 一 个 工程 目录 ， 在 其 下 
面 建立 如 下 文件 夹 来 组 织 代 码 文件 : 


1.bin 用 来 存放 可 执行 的 二 进 制 文件 。 


2.include/myslam 人 存放 SLAM 模 块 的 头 文 件 ， 主 要 是 .h 文 件 。 这 种 做 法 
的 理由 是 ， 当 把 包含 目录 设 到 include， 引 用 自己 的 头 文件 时 ， 需 要 写 
include"myslam/xxx.h", Xt#FA AMA NEHA. 


3.src 存 放 源 代码 文件 ， 主 要 是 .cpp 文 件 。 
4.test 存 放 测 试用 的 文件 ， 也 是 .cpp 文 件 。 


5.lib 存 放 编 译 好 的 库 文 件 。 
6.con fi g 存 放 配 置 文件 。 


7.cmake_modules 第 三 方 库 的 cmake 文 件 ， 在 使 用 g2o 之 类 的 库 时 会 用 
到 它 。 

以 上 就 是 我 们 的 目录 结构 ， 如 图 9-2 所 示 。 相 比 于 之 前 每 一 讲 内 零 零 
散 散 地 放 着 的 main.cpp， 这 种 做 法 显得 更 有 条 理 。 接 下 来 ， 我 们 会 在 这 些 
目录 里 不 断 地 添加 新 文件 ， 了 逐渐 形成 一 个 完整 的 程序 。 


> 33K sources slambook project 0.1 


名 最 近 使 用 的 
CEE bin CMakeLists.txt cmake_modules config 


口 桌面 — 


口 document 


include lib src test 
® download 


JI music 


=} picture 

Ch video 

E 回收 站 

设备 

D 计算 机 
网 络 

& 浏览 网 络 

Q 连接 到 服务 器 


图 9-2 ”工程 项 目的 目录 。 


9.1.2 ”确定 基本 数据 结构 


为 了 让 程序 跑 起 来 ， 我 们 要 设计 好 数据 单元 ， 以 及 程序 处 理 的 流 
程 。 这 好 比 构成 房屋 的 一 个 个 的 柱子 和 砖 块 。 那 么 ， 在 一 个 SLAM 程 序 
中 ， 有 哪些 结构 是 最 基本 的 呢 ? 我们 抽象 出 以 下 基本 概念 : 


1. 帧 : 帧 是 相机 采集 到 的 图 像 单位 。 它 主要 包含 一 个 图 像 (RGB-D 
情形 下 是 一 对 图 像 ，。 此 外 ， 还 有 特征 点 、 位 姿 、 内 参 等 信息 。 在 视觉 
SLAM 中 我 们 会 谈论 关键 帆 (Key-frame) 。 由 于 相机 采集 的 数据 很 多 ， 
存储 所 有 的 数据 显然 是 不 现实 的 。 否 则 ， 如 果 相机 放 在 桌 上 不 动 ， 程 序 


的 内 存 占用 也 会 越 来 越 高 直至 无 法 接受 。 通 常 的 做 法 是 把 某 些 我 们 认为 
更 重要 的 帧 保存 起 来 ， 并 认为 相机 轨迹 可 以 用 这 些 关 键 帧 来 描述 。 关 键 
帧 如 何 选 择 是 一 个 很 大 的 问题 ， 而 且 基 于 工程 经 验 ， 很 少 有 理论 上 的 指 
导 。 在 本 书 中 我 们 会 使 用 一 个 关键 帧 选择 方法 ， 但 读者 亦 可 考虑 上 自己 提 
出 新 的 方式 。 


2. 路 标 : 路 标点 即 图 像 中 的 特征 点 。 在 相机 运动 后 ， 我 们 还 能 估计 
它们 的 3D 位 置 。 通 常 ， 会 把 路 标点 放 在 一 个 地 图 当中 ， 并 将 新 来 的 帧 与 
地 图 中 的 路 标点 进行 匹配 ， 估 计 相 机 位 姿 。 


帧 的 位 姿 与 路 标的 位 置 估 计 相 当 于 一 个 局 部 的 SLAM 问 题 。 除 此 之 
外 ， 我 们 还 需要 一 些 工具 ， 让 程序 写 起 来 更 流畅 。 例 如 


1. 配 置 文件 : 在 写 程序 过 程 中 你 会 经 常 遇 到 各 种 各 样 的 参数 ， 比 
如 ， 相 机 的 内 参 、 特 征 点 的 数量 、 匹 配 时 选择 的 比例 ， 等 等 。 你 可 以 把 
这 些 数 写 在 程序 中 ， 但 那 不 是 一 个 好 习惯 。 你 会 经 单 修改 这 些 参数， 但 
每 次 修改 后 都 要 重新 编译 一 遍 程序 。 当 其 数量 越 来 越 多 时 ， 修 改 就 变 得 
越 来 越 困难 。 所 以 ， 更 好 的 方式 是 在 外 部 定义 一 个 配置 文件 ， 程 序 运 行 
时 读 取 该 配置 文件 中 的 参数 值 。 这 样 ， 每 次 只 要 修改 配置 文件 内 容 就 行 
了 ， 不 必 对 程序 本 身 做 任何 修改 。 


2. 坐 标 变换 : 你 会 经 常 需要 在 坐标 系 间 进行 坐标 变换 ， 例 如 ， 世 界 
坐标 到 相机 坐标 、 相 机 坐标 到 归 一 化 相机 坐标 、 归 一 化 相机 坐标 到 像素 
坐标 ， 等 等 。 定 义 一 个 类 把 这 些 操作 都 放 在 一 起 将 更 方便 。 


下 面 我 们 就 来 定义 帧 、 路 标 这 几 个 概念 ， 在 C++ 中 都 以 类 来 表示 。 我 
们 尽量 保证 一 个 类 有 单独 的 头 文件 和 源 文 件 ， 避 免 把 许多 个 类 放 在 同一 
个 文件 中 。 然 后 ， 把 函数 声明 放 在 头 文件 ， 实 现 放 在 源 文 件 中 (BRIER 
数 很 短 ， 也 可 以 写 在 头 文 件 中 ) 。 我 们 参照 Google 的 命名 规范 ， 同 时 考 
虑 尽量 以 初学 者 也 能 看 懂 的 方式 来 写 程序 。 由 于 我 们 的 程序 是 偏向 算法 
而 非 软 件 工程 的 ， 所 以 不 讨论 复杂 的 类 继承 关系 、 接 口 、 模 板 等 ， 而 更 
关注 算法 的 正确 实现 ， 以 及 是 否 便于 扩展 。 我 们 会 把 数据 成 员 设 置 为 公 
有 ， 尽 管 这 在 C++ 软件 设计 中 是 应 该 避免 的 ， 如 果 读 者 愿意 ， 也 可 以 把 它 
们 改 成 private 或 protected 接 口 ， 并 添加 设置 和 获取 接口 。 在 过 程 较为 复杂 
的 算法 中 ， 我 们 会 把 它 分 解 成 若干 步 又， 例如 特征 提取 和 匹配 应 该 分 别 
在 不 同 的 水 数 中 实现 ， 这 样 ， 当 我 们 想 修 改 算 法 流程 时 ， 就 不 需要 修改 
整个 运行 流程 ， 只 需 调 整 局 部 的 处 理 方式 即 可 。 


现在 ， 让 我 们 开始 写 VO。 我 们 把 这 个 版 本 定 为 0.1 版 ， 表 示 这 是 刚 开 
始 的 阶段 。 我 们 一 共 写 5 个 类 : Frame 为 帧 ，Camera 为 相机 模型 ， 
MapPoint 为 特征 点 /路 标点 ，Map 管 理 特征 点 ，Con fi g 提 供 配置 参数 。 它 
们 的 关系 如 图 9-3 所 示 。 我 们 现在 只 写 它们 的 数据 成 员 和 常用 方法 ， 而 在 
后 面 用 到 更 多 内 容 时 再 行 添加 。 


Camera 


图 9-3 ”基本 类 的 关系 示意 图 。 


Camera 类 最 简单 ， 我 们 先 来 实现 它 。 


Has many 


— > Has a 
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Config 


9.1.3 Camera 类 


Camera 类 存储 相机 的 内 参 和 外 参 ， 并 完成 相机 坐标 系 、 像 素 坐 标 系 
和 世界 坐标 系 之 间 的 坐标 变换 。 当 然 ， 在 世界 坐标 系 中 你 需要 一 个 相机 
的 (变动 的 ) 外 参 ， 我 们 以 参数 的 形式 传 入 。 


由 slambook/project/0.1/include/myslam/camera.h 


1 | #ifndef CAMERA_H 

#define CAMERA_H 
3 |#include "myslam/common_include.h" 
4 | namespace myslam 

{ 
6 |// Pinhole RGB-D camera model 
7 | class Camera 
s |{ 
9 | public: 
10 typedef std::shared_ptr<Camera> Ptr; 
11 float fx_, fy_, cx_, cy_, depth_scale_; // Camera intrinsics 
12 
1B Camera() ; 
14 Camera ( float fx, float fy, float cx, float cy, float depth_scale=0 ) : 
15 fx_ ( fx ), fy_ ( fy ), cx ( cx ), cy- ( cy ), depth_scale_ ( depth_scale ) 
16 {} 
17 
18 // coordinate transform: world, camera, pizel 
19 Vector3d world2camera( const Vector3d& p_w, const SE3& T_c_w ); 
20 Vector3d camera2world( const Vector3d& p_c, const SE3& T_c_w ); 
21 Vector2d camera2pixel( const Vector3d& p_c ); 
22 Vector3d pixel2camera( const Vector2dk p_p, double depth=1 ); 
23 Vector3d pixel2world ( const Vector2d& p_p, const SE3& T_c_w, double depth=1 ); 
24 Vector2d world2pixel ( const Vector3d& p_w, const SE3& T_c_w ); 
2s |J; 
26 | } 
27 |#endif // CAMERA_H 


说 明 如 下 (由 上 往 下 ) : 


1. 在 这 个 简单 的 例子 中 ， 我 们 给 出 了 防止 头 文件 重复 引用 的 ifndef 宏 
定义 。 如 果 没 有 这 个 宏 ， 在 两 处 引用 此 头 文 件 时 将 出 现 类 的 重复 定义 。 
所 以 ， 在 每 个 程序 头 文件 里 都 会 定义 这 样 一 个 宏 。 


2. 我 们 用 命名 空间 namespace myslam 将 类 定义 包 焉 起 来 (因为 是 我 们 
自己 写 的 SLAM， 所 以 命名 空间 就 叫 myslam 了 ) 。 命 名 空间 可 以 防止 我 
们 不 小 心 定义 出 别 的 库 里 同名 的 函数 ， 也 是 一 种 比较 安全 和 规范 的 做 
法 。 由 于 安定 义 和 命 名 空间 在 每 个 文件 中 都 会 写 一 遍 ， 所 以 我 们 只 在 这 
里 稍 加 介绍 ， 后 面 将 略 去 。 


3. 我 们 把 一 些 常用 的 头 文 件 放 在 common _ include.h 文 件 中 ， 这 样 就 可 
以 避免 每 次 书写 很 长 的 一 串 include。 

4. 我 们 把 智能 指针 定义 成 Camera 的 指针 类 型 ， 因 此 以 后 在 传递 参数 时 
使 用 Camera::Ptr 类 型 即 可 。 

5. 我 们 用 Sophus::SE3 来 表达 相机 的 位 姿 。Sophus 库 在 李 代 数 一 讲 介绍 
过 。 

在 源 文件 中 ， 给 出 Camera 方 法 的 实现 : 


内 slambook/project/0.1/src/camera.cpp 


#include "myslam/camera.h" 


namespace myslam 


{ 


Camera: : Camera() 
{ 
} 


Vector3d Camera::world2camera ( const Vector3d& p_w, const SE3& T_c_w ) 


{ 


return T_c_w*p_w; 


Vector3d Camera::camera2world ( const Vector3d& p_c, const SE3& T_c_w ) 


{ 


return T_c_w.inverse() *p_c; 


Vector2d Camera::camera2pixel ( const Vector3d& p_c ) 


{ 
return Vector2d ( 
fx_ * p c (0,0 ) / p_c ( 2,0 ) + cx_, 
fy_ * ple ( 1,0 ) / ple ( 2,0: ) + ey 
) ; 
} 


Vector3d Camera::pixel2camera ( const Vector2d& p_p, double depth ) 
{ 
return Vector3d ( 
( p p ( 0,0 )-cx_ ) *depth/fx_, 
( p p ( 1,0 )-cy_ ) *depth/fy_, 
depth 
); 


Vector2d Camera::world2pixel ( const Vector3d& p_w, const SE3& T_c_w ) 


37 | { 
38 return camera2pixel ( world2camera ( p_w, T_c_w ) ); 


39 } 


41 | Vector3d Camera::pixel2world ( const Vector2d& p_p, const SE3& T_c_w, double depth 
) 
2 | { 
43 return camera2world ( pixel2camera ( p_p, depth ), T_c_w ); 
4 |} 
45 } 


读者 可 以 对 照 一 下 这 些 方 法 是 否 和 第 5 讲 的 内 容 一 致 。 它 们 完成 了 像 
素 坐 标 系 、 相 机 坐标 系 和 世界 坐标 系 间 的 坐标 变换 。 


9.1.4 Frame 类 


下 面 来 考虑 Frame 类 。 Frame 类 是 基本 数据 单元 ， 在 许多 地 方 会 用 
到 ， 但 现在 是 初期 设计 阶段 ， 我 们 还 不 清楚 以 后 可 能 新 加 的 内 容 。 所 以 
这 里 的 Frame 类 只 提供 基本 的 数据 存储 和 接口 。 如 果 之 后 有 新 增 的 内 容 ， 
再 继续 往 里 添加 。 


内 slambook/project/0.1/include/myslam/frame.h 


-一 E. J A 5 = 4 A 
En a ee E a a 
1 |class Frame 


a rt 

3 | public: 

4 typedef std::shared_ptr<Frame> Ptr; 

5 unsigned long id_; // id of this frame 

6 double time_stamp_; // when it is recorded 

7 SE3 T_c_w_; // transform from world to camera 

8 Camera::Ptr camera_; // Pinhole RGB-D Camera model 
9 Mat color_, depth_; // color and depth image 


11 |public: // data members 


12 Frame () ; 
13 Frame( long id, double time_stamp=0, SE3 T_c_w=SE3(), Camera::Ptr camera= 
nullptr, Mat color=Mat(), Mat depth=Mat() ); 
14 ~Frame() ; 
15 
16 // factory function 
17 static Frame::Ptr createFrame() ; 
18 
19 // find the depth in depth map 
20 double findDepth( const cv::KeyPoint& kp ); 
21 
22 // Get Camera Center 
2B Vector3d getCamCenter() const; 
24 
25 // check if a point is in this frame 
26 bool isInFrame( const Vector3d& pt_world ); 
27 |}; 


在 Frame 中 ， 我 们 定义 了 ID、 时 间 戳 、 位 资 、 相 机 、 图 像 这 几 个 量 ， 
这 应 该 是 一 个 帧 当中 含有 的 最 重要 的 信息 。 在 方法 中 ， 我 们 提取 了 几 个 
重要 的 方法 : 创建 Frame、 和 寻找 给 定点 对 应 的 深度 、 获 取 相 机 光 心 、 判 断 
某 个 点 是 否 在 视野 内 ， 等 等 。 它 们 的 实现 是 比较 平凡 的 ， 请 读者 参考 
frame.cpp 了 解 这 些 国 效 的 具体 实现 。 


9.1.5 “MapPoint 类 


MapPoint 表 示 路 标点 。 我 们 将 估计 它 的 世界 坐标 ， 并 且 会 拿 当前 帧 
提取 到 的 特征 点 与 地 图 中 的 路 标点 匹配 ， 来 估计 相机 的 运动 ， 因 此 还 需 
要 存储 它 对 应 的 描述 子 。 此 外 ， 我 们 会 记录 一 个 点 被 观测 到 的 次 数 和 被 
匹配 的 次 数 ， 作 为 评价 其 好 坏 程 度 的 指标 。 


四 slambook/project/0.1/include/myslam/frame.h 


1 |class MapPoint 

2 |{ 

3 | public: 

4 typedef shared_ptr<MapPoint> Ptr; 

5 unsigned long id_; // ID 

6 Vector3d pos_; // Position in world 

“i Vector3d norm_; // Normal of viewing direction 

8 Mat descriptor_; // Descriptor for matching 

9 int observed_times_; // being observed by feature matching algo. 
10 int correct_times_; // being an inliner in pose estimation 
11 

12 MapPoint (); 

13 MapPoint( long id, Vector3d position, Vector3d norm ) ; 

14 

15 // factory function 

16 static MapPoint::Ptr createMapPoint(); 

7 |}; 


同样 ， 读 者 可 以 浏览 srmap.cpp 查 看 其 实现 。 目 前 为 止 我 们 只 考虑 这 
些 数据 成 员 的 初始 化 问题 。 


9.1.6 Map 类 


Map 类 管理 着 所 有 的 路 标点 ， 并 负责 添加 新 路 标 、 删 除 不 好 的 路 标 等 
工作 。VO 的 匹配 过 程 只 要 和 Map 打 交道 即 可 。 当 然 Map 也 会 有 很 多 操 
作 ， 但 现 阶段 我 们 只 定义 主要 的 数据 结构 。 


由 slambook/project/0.1/include/myslam/map.h 


1 |class Map 

2 jd 

3 | public: 

4 typedef shared_ptr<Map> Ptr; 

5 unordered_map<unsigned long, MapPoint::Ptr > map_points_; // all 
landmarks 

6 unordered_map<unsigned long, Frame::Ptr > keyframes_; // all key- 
frames 

7 

8 Map() {} 

9 

10 void insertKeyFrame( Frame::Ptr frame ); 

ul void insertMapPoint( MapPoint::Ptr map_point ); 

12 |}; 

Map 类 中 实际 存储 了 各 个 关键 帧 和 路 标点 ， 既 需要 随机 访问 ， 又 需 


随时 插入 和 删除 ， 因 此 我 们 使 用 散 列 (Hash) 来 进 井 行人 存储 。 


9.1.7 Con fi g% 


Con fi g 类 负责 参数 文件 的 读 取 ， 并 在 程序 的 任意 地 方 都 可 随时 提供 
参数 的 值 。 所 以 我 们 把 Con fi g 写 成 单 件 模式 (Singleton) 。 它 只 有 一 个 
全 局 对 象 ， 当 我 们 设置 参数 文件 时 ， 创 建 该 对 象 并 读 取 参数 文件 ， 随后 
就 可 以 在 任意 地 方 访问 参数 值 ， 最 后 在 程序 结束 时 上 自动 销毁 。 


由 slambook/project/0.1/include/myslam/con fi g.h 


class Config 


1 

z |< 

3 | private: 

4 static std::shared_ptr<Config> config_; 

5 cv::FileStorage file_; 

6 

7 Config () {} // private constructor makes a singleton 
8 | public: 

9 ~Config(); // close the file when deconstructing 


11 // set a new config file 


12 static void setParameterFile( const std::string& filename ); 
13 
14 // access the parameter values 
15 template< typename T > 
16 static T get( const std::string& key ) 
17 { 
18 return T( Config::config_->file_[key] ); 
19 } 
20 +3 
说 明 如 下 : 


1. 我 们 把 构造 辫 数 声明 为 私有 ， 防 止 这 个 类 的 对 象 在 别处 建立 ， 


能 在 setParameterFile 时 构造 。 实 际 构 造 的 对 象 是 Con fi g 的 智能 指针 : 
static shared_ptr<Con fi g>con fi g 。 用 智能 指针 的 原因 是 可 以 自动 析 构 ， 


省 得 我 们 再 调 一 个 别 的 函数 来 做 析 构 。 


2. 在 文件 读 取 方面 ， 我 们 使 用 OpenCV 提 供 的 FileStorage 类 。 它 可 以 
读 取 一 个 YAML 文 件 ， 且 可 以 访问 其 中 任意 一 个 字段 。 由 于 参数 实质 值 可 
能 为 整数 、 浮 点 数 或 字符 串 ， 所 以 我 们 通过 一 个 模板 函数 get 来 获得 任意 


类 型 的 参数 值 。 


下 面 是 Con fi g 的 实现 。 注 意 ， 我 们 把 单 例 模式 的 全 局 指针 定义 在 此 


源 文件 中 了 : 
四 slambook/project/0.1/src/con fi g.cpp 


void Config::setParameterFile( const std::string& filename ) 
{ 
if ( config_ == nullptr ) 
config_ = shared_ptr<Config>(new Config) ; 
config_->file_ = cv::FileStorage( filename.c_str(), cv::FileStorage::READ ); 
if ( config_->file_.isOpened() == false ) 
{ 


std::cerr<<"parameter file "<<filename<<" does not exist."<<std::end1l; 


config_->file_.release() ; 
return ; 
} 
} 
Config: :~Config() 
{ 
if ( file_.isOpened() ) 
file_.release(); 
} 
shared_ptr<Config> Config::config_ = nullptr; 


在 实现 中 ， 我 们 只 要 判断 一 下 参数 文件 是 否 存在 即 可 。 定 义 了 这 个 
Con fi g 类 后 ， 我 们 可 以 在 任何 地 方 获取 参数 文件 里 的 参数 。 例 如 ， 当 想 
要 定义 相机 的 焦距 / 时 ， 按 照 如 下 步骤 操作 即 可 : 

1. 在 参数 文件 中 加 入 : “Camera.fx:500”。 

2. 在 代码 中 使 用 : 


1 | myslam::Config::setParameterFile("parameter.yaml") ; 


2 | double fx = myslam::Config::get<double> ("Camera.fx"); 


束 能 获得 f 的 值 了 。 


当然 ， 参 数 文件 的 实现 方法 绝对 不 止 这 一 种 。 我 们 主要 从 程序 开发 
上 的 便利 角度 来 考虑 这 个 实现 ， 读 者 当然 也 可 以 用 更 简单 的 方式 来 实现 
参数 的 配置 。 


至 此 ， 我 们 定义 了 SLAM 程 序 的 基本 数据 结构 ， 书 写 了 若干 个 基本 
类 。 这 好 比 是 造 房子 的 砖头 和 水 泥 。 你 可 以 调用 cmake 编 译 这 个 0.1 版 ， 尽 


~ 


管 它 还 没有 实质 性 的 功能 。 接 下 来 我 们 来 考虑 把 前 面 讲 过 的 VO 算法 加 到 


工程 中 ， 并 做 一 些 测试 来 调整 各 算法 的 性 能 。 注 意 ， 笔 者 会 刻意 地 暴露 
某 些 设计 的 问题 ， 所 以 你 看 到 的 实现 不 见得 就 是 最 好 的 (或 者 足够 好 
AY) o 


9.2 基本 的 VO: 特征 提取 和 匹配 


下 面 我 们 来 实现 VO， 先 来 考虑 特征 点 法 。 它 的 任务 是 ， 根 据 输 入 的 
图 像 ， 计 算 相 机 运动 和 特征 点 位 置 。 前 面 我 们 讨论 的 都 是 在 两 两 帧 间 的 
位 姿 估 计 ， 然 而 我 们 将 发 现 ， 仅 赁 两 帧 的 估计 是 不 够 的 。 我 们 会 把 特征 
点 缓存 成 一 个 小 地 图 ， 计 算 当 前 帧 与 地 图 之 间 的 位 置 关系 。 但 那样 程序 
会 复杂 一 些 ， 所 以 ， 让 我 们 先 订 个 小 目标 ”暂时 从 两 两 帧 问 的 运动 估计 
出 发 。 


9.2.1 ”两 两 帧 的 视觉 里 程 计 


如 果 像 前 面 两 讲 一 样 ， 只 关心 两 个 帧 之 间 的 运动 估计 ， 并 且 不 优化 
特征 点 的 位 置 。 然 后 把 估 得 的 位 姿 *" 串 ”起 来 ， 也 能 得 到 一 条 运动 轨迹 。 
这 种 方式 可 以 看 成 两 两 帧 间 的 (Pairwise) 无 结构 (Structureless) 的 
VO， 实 现 起 来 最 为 简单 ， 但 是 效果 不 佳 。 为 什么 不 佳 呢 ? 我 们 一 起 来 体 
验 一 下 。 记 该 工程 为 0.2 版 本 。 

两 两 帧 之 间 的 VO 工作 示意 图 如 图 9-4 所 示 。 在 这 种 VO 里 ， 我 们 定义 
了 参考 帧 (Reference) 和 当前 帧 (Current) 这 两 个 概念 。 以 参考 帧 为 坐 
标 系 ， 我 们 把 当前 帧 与 它 进行 特征 匹配 ， 并 估计 运动 关系 。 假 设 参考 帧 
相对 世界 坐标 的 变换 矩阵 为 了 ， 当 前 帧 与 世界 坐标 系 间 为 T, ， 则 待 估计 


的 运动 与 这 两 个 帧 的 变换 矩阵 构成 左 乘 关系 : 


Ter, st. Tew = Ter Trw. 


在 t- 1 到 上 时刻， 我 们 以 上 1 为 参考 ， 求 取 t 时 刻 的 运动 。 这 可 以 通过 特 
征 点 匹配 、 光 流 或 直接 法 得 到 ， 但 这 里 我 们 只 关心 运动 ， 不 关心 结构 。 
换 句 话说 ， 只 要 通过 特征 点 成 功 求 出 了 运动 ， 我 们 就 不 再 需要 这 一 帧 的 
FHER T o 这 种 做 法 当然 会 有 缺陷 ， 但 是 忽略 掉 数 量 庞大 的 特征 点 可 以 
节省 许多 的 计算 量 。 然 后 ， 在 t 到 t +1 时 刻 ， 我 们 又 以 t 时 刻 为 参考 帧 ， 考 
ret 到 t +1 间 的 运动 关系 。 如 此 往复 ， 就 得 到 了 一 条 运动 轨迹 。 


N on// N 7 N 7 


} } } 
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图 9-4 ”两 两 帧 的 VO 示 意图 。 


这 种 VO 的 工作 方式 是 简单 的 ， 不 过 实现 也 可 以 有 若干 种 。 我 们 以 传 
统 的 匹配 特征 点 一 一 求 PnP 的 方法 为 例 实现 一 遍 。 希 望 读 者 能 够 结合 之 前 
几 讲 的 知识 ， 目 己 实现 一 下 光 流 /直接 法 或 1CP 求 运动 的 VO。 在 匹配 特征 
点 的 万 式 中 ， 最 重要 的 是 参考 帧 与 当前 帧 之 间 的 特征 匹配 关系 ， 它 的 流 
程 可 归纳 如 下 : 


1. 对 新 来 的 当前 帧 ， 提 取 关 键 点 和 描述 子 。 


2. 如 果 系 统 未 初始 化 ， 以 该 帧 为 参考 帆 ， 根 据 深 度 图 计算 关键 点 的 
3D 位 置 ， 返 回 第 1 步 。 


3. 估 计 参 考 帧 与 当前 帧 间 的 运动 。 
4. 判 断 上 述 估计 是 否 成 功 。 
5. 各 成 功 ， 把 当前 帧 作为 新 的 参考 帧 ， 返 回 第 1 步 。 


6. 若 失败 ， 计 录 连 续 丢 失 帧 数 。 当 连续 丢失 超过 一 定 帧 数 时 ， 置 VO 
状态 为 丢失 ， 算 法 结束 。 各 未 超过 ， 返 回 第 1 步 。 


VisualOdometry 类 给 出 了 上 述 算法 的 实现 。 


内 slambook/project/0.2/include/myslam/visual_odometry.h 


class Visual0dometry 
{ 
public: 


typedef shared_ptr<Visual0dometry> Ptr; 
enum VOState { 

INITIALIZING=-1, 

OK=0, 

LOST 
}; 


11 VOState state_; // current VO status 

12 Map: :Ptr map_; // map with all frames and map points 

13 Frame::Ptr ref_; // reference frame 

14 Frame::Ptr curr_; // current frame 

15 

16 cv::Ptr<cv::0RB> orb_; // orb detector and computer 

17 vector<cv: :Point3f> pts_3d_ref_; // 3d points in reference frame 
18 vector<cv::KeyPoint> keypoints_curr_; // keypoints in current frame 
19 Mat descriptors_curr_; // descriptor in current frame 

20 Mat descriptors_ref_; // descriptor in reference frame 

21 vector<cv::DMatch> feature_matches_; 

22 

23 SE3 T_c_r_estimated_; // the estimated pose of current frame 

24 int num_inliers_; // number of inlier features in icp 

25 int num_lost_; // number of lost times 

26 

27 // parameters 

28 int num_of_features_; // number of features 

29 double scale_factor_; // scale in image pyramid 

30 int level_pyramid_; // number of pyramid levels 

31 float match_ratio_; // ratio for selecting good matches 

32 int max_num_lost_; // max number of continuous lost times 

33 int min_inliers_; // minimum inliers 

34 

35 double key_frame_min_rot; // minimal rotation of two key-frames 
36 double key_frame_min_trans; // minimal translation of two key-frames 


33 | public: // functions 


39 VisualOdometry(); 

40 ~VisualOdometry() ; 

41 

42 bool addFrame( Frame::Ptr frame ); // add a new frame 


44 | protected: 


45 // inner operation 

46 void extractKeyPoints() ; 
47 void computeDescriptors(); 
48 void featureMatching(); 

49 void poseEstimationPnP(); 
50 void setRef3DPoints(); 

51 

52 void addKeyFrame() ; 

53 bool checkEstimatedPose() ; 


54 bool checkKeyFrame(); 


关于 这 个 VisualOdometry 类 ， 有 几 点 需要 解释 : 


1.VO 本 身 有 若干 种 状态 : 设 定 第 一 帧 、 顺 利 跟踪 或 丢失 ， 你 可 以 把 
它 看 成 一 个 有 限 状 态 机 (Finite State Machine, FSM) 。 当 然 状态 也 可 以 
有 更 多 种 ， 例 如 ， 单 目 VO 至 少 还 有 一 个 初始 化 状态 。 在 我 们 的 实现 中 ， 
考虑 最 简单 的 三 个 状态 : Meth. IER, BK. 

2. 我 们 把 一 些 中 间 变 量 定 义 在 类 中 ， 这 样 可 省 去 复杂 的 参数 传递 。 
为 它们 都 是 定义 在 类 内 部 的 ， 所 以 各 个 函数 都 可 以 访问 它们 。 

3. 特 征 提取 和 匹配 当中 的 参数 从 参数 文件 中 读 取 。 例 如 : 


1 | VisualOdometry::VisualOdometry() : 

2 state_ ( INITIALIZING ), ref_ ( nullptr ), curr_ ( nullptr ), map_ ( 
new Map ), num_lost_ ( 0 ), num_inliers_ ( 0 ) 

a lit 

4 num_of_features_ = Config::get<int> ( "number_of_features" ); 

5 scale_factor_ = Config::get<double> ( "scale_factor" ); 

6 level_pyramid_ = Config::get<int> ( "level_pyramid" ); 

7 match_ratio_ = Config::get<float> ( "match_ratio" ); 

8 

9 |} 


4.addFrame 闵 数 是 外 部 调用 的 接口 。 使 用 VO 时 ， 将 图 像 数 据 装 入 
Frame 类 后 ， 调 用 addFrame 估 计 其 位 资 。 该 函数 根据 VO 所 处 的 状态 实现 
不 同 的 操作 : 


bool VisualOdometry::addFrame ( Frame::Ptr frame ) 


4. 


switch ( state_ ) 


{ 

case INITIALIZING: 

{ 
state_ = OK; 
curr_ = ref_ = frame; 
map_->insertKeyFrame ( frame ); 
// extract features from first frame 
extractKeyPoints() ; 
computeDescriptors() ; 
// compute the 3d position of features in ref frame 
setRef3DPoints() ; 
break; 

} 

case OK: 

{ 
curr_ = frame; 
extractKeyPoints(); 


computeDescriptors() ; 


22 featureMatching() ; 

23 poseEstimationPnP () ; 

24 if ( checkEstimatedPose() == true ) // a good estimation 

25 { 

26 curr_->T_c_w_ = T_c_r_estimated_ * ref_->T_c_w_; // T_c_w = 
T_c_r*T_r_w 

27 ref_ = curr_; 

28 setRef3DPoints() ; 

29 num_lost_ = 0; 

30 if ( checkKeyFrame() == true ) // is a key-frame 

31 { 

32 addKeyFrame () ; 

33 } 

34 } 

35 else // bad estimation due to various reasons 

36 { 

37 num_lost_++; 

38 if ( num_lost_ > max_num_lost_ ) 

39 { 

40 state_ = LOST; 

41 } 

42 return false; 

43 ¥ 

44 break; 

45 } 

46 case LOST: 

47 { 

48 cout<<"vo has lost."<<endl; 

49 break; 

50 } 

51 } 

52 return true; 

53 } 


值得 一 提 的 是 ， 由 于 各 种 原因 ， 我 们 设计 的 上 述 VO 算 法 ， 每 一 步 都 
有 可 能 失败 。 例 如 ， 图 片 中 不 易 提 特征 、 特 征 点 缺少 深度 值 、 误 匹配 、 
运动 估计 出 错 ， 等 等 。 因 此 ， 要 设计 一 个 健壮 的 VO， 必 须 (最 好 是 显 式 
地 ) 考虑 到 上 述 所 有 可 能 出 错 的 地 方 一 一 那 自然 会 使 程序 变 得 非常 复 
杂 。 我 们 在 checkEstimatedPose 中 根据 内 点 (inlier) 的 数量 及 运动 的 大 小 


做 一 个 简单 的 检测 : 认为 内 点 不 可 太 少 ， 而 运动 不 可 能 过 大 。 当 然 ， 读 
者 也 可 以 思考 其 他 检测 问题 的 手段 ， 尝 试 一 下 效果 。 
我 们 略 去 VisualOdometry 类 其 余 的 实现 ， 读 者 可 在 GitHub 上 找到 所 有 


的 源 代 码 。 最 后 ， 我 们 在 test 中 加 入 该 VO 的 测试 程序 ， 使 用 效 据 集 观察 估 
计 的 运动 效果 : 


由 slambook/project/0.2/test/run_vo.cpp 


43 


int main ( int argc, char** argv ) 


{ 


if ( argc != 2 ) 
{ 
cout<<"usage: run_vo parameter_file"<<endl; 


return 1; 


myslam::Config::setParameterFile ( argv[i] ); 


myslam::VisualOdometry::Ptr vo ( new myslam::VisualOdometry ); 


string dataset_dir = myslam::Config::get<string> ( "dataset_dir" ); 
cout<<"dataset: "<<dataset_dir<<endl; 
ifstream fin ( dataset_dirt+"/associate.txt" ); 
if ( !fin ) 
{ 
cout<<"please generate the associate file called associate.txt!"<<endl; 


return 1; 


vector<string> rgb_files, depth_files; 

vector<double> rgb_times, depth_times; 

while ( !fin.eof() ) 

{ 
string rgb_time, rgb_file, depth_time, depth_file; 
fin>>rgb_time>>rgb_file>>depth_time>>depth_file; 
rgb_times.push_back ( atof ( rgb_time.c_str() ) ); 
depth_times.push_back ( atof ( depth_time.c_str() ) ); 
rgb_files.push_back ( dataset_dir+"/"+rgb_file ); 
depth_files.push_back ( dataset_dirt+"/"+depth_file ); 


if ( fin.good() == false ) 


break; 


myslam::Camera::Ptr camera ( new myslam::Camera ); 


// visualization 

cv::viz::Viz3d vis("Visual Odometry") ; 

cv::viz::WCoordinateSystem world_coor(1.0), camera_coor(0.5); 

cv::Point3d cam_pos( 0, -1.0, -1.0 ), cam_focal_point(0,0,0), cam_y_dir(0,1,0); 
cv::Affine3d cam_pose = cv::viz::makeCameraPose( cam_pos, cam_focal_point, 
cam_y_dir ); 


vis.setViewerPose( cam_pose ); 


75 


76 


77 


78 


79 


80 


81 


82 


83 


84 


world_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 2.0); 


camera_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 1.0); 


vis.showWidget( "World", world_coor ); 


vis.showWidget( "Camera", camera_coor ); 


cout<<"read total "<<rgb_files.size() <<" entries"<<end1; 


for ( int i=0; i<rgb_files.size(); i++ ) 


{ 


Mat color = cv::imread ( rgb_files[i] ); 


Frame: :createFrame() ; 


Mat depth = cv::imread ( depth_files[i], -1 ); 

if ( color.data==nullptr || depth.data==nullptr ) 
break; 

myslam::Frame::Ptr pFrame = myslam:: 
pFrame->camera_ = camera; 

pFrame->color_ = color; 


pFrame->depth_ = depth; 


pFrame->time_stamp_ = rgb_times[i]; 


boost::timer timer; 


vo->addFrame ( pFrame ); 


cout<<"VO costs time: "<<timer.elapsed()<<end1l; 


if ( vo->state_ == myslam::VisualOdometry::LOST ) 


break; 


SE3 Tcw = pFrame->T_c_w_.inverse(); 


// show the map and the camera pose 
cv: :Affine3d M( 
cv: :Affine3d: :Mat3( 
Tcw.rotation_matrix() (0,0), 
rotation_matrix() (0,2), 
Tcow.rotation_matrix() (1,0), 
rotation_matrix() (1,2), 
Tcw.rotation_matrix() (2,0), 


rotation_matrix() (2,2) 


cv: :Affine3d: :Vec3( 
Tcw.translation() (0,0), Tew. 
(2,0) 


cv::imshow("image", color ); 
cv::waitKey (1); 


vis.setWidgetPose( "Camera", M); 


Tcew.rotation_matrix()(0,1), Tew. 


Tcw.rotation_matrix() (1,1), Tew. 


Tcw.rotation_matrix() (2,1), Tew. 


translation()(1,0), Tcw.translation() 


85 vis.spinOnce(1, false); 


86 } 
87 return 0; 
s |} 


为 了 运行 这 个 程序 ， 需 要 做 几 件 事 : 

1. 因 为 我 们 用 OpenCV 3 的 viz 模 块 显示 估计 位 姿 ， 请 确保 你 安装 的 是 
OpenCV 3， 并 且 viz 模 块 也 已 编译 安装 。 

2. 准 备 TUM 数 据 集中 的 其 中 一 个 。 简 单 起 见 ， 笔 者 推荐 ff1_xyz 那 一 
个 。 请 使 用 associate.py 生 成 一 个 配对 文件 associate.txt。 天 于 TUM 数 据 集 
格式 ， 在 8.3 节 中 已 介绍 。 


3. 在 con fi g/default.yaml 中 填写 你 的 数据 集 所 在 路 径 ， 参 照 笔 者 的 写 
法 即 可 。 然 后 ， 用 


1 | bin/run_vo config/default.yaml | 


执行 程序 ， 就 可 以 看 到 实时 的 演示 了， 如 图 9-5 所 示 。 


在 演示 程序 中 ， 你 可 以 看 到 当前 帧 的 图 像 与 它 的 估计 位 置 。 我 们 画 
出 了 世界 坐标 系 的 坐标 轴 (A) 与 当前 帧 的 坐标 轴 (小 ) ， 颜 色 与 轴 的 
对 应 关系 为 : 蓝 色 一 Zz， 红 色 一 X， 绿 色 一 Y。 你 可 以 直观 地 感受 到 相机 
的 运动 ， 它 与 我 们 人 类 的 感觉 是 大 致 相符 的 ， 尽 管 效 果 距 离 预 想 还 有 一 
定 的 差距 。 程 序 还 输出 了 VO 单 次 计算 的 用 时 ， 在 笔者 的 机 器 上 ， 能 够 以 
每 次 花费 30ms 左 右 的 速度 运行 。 减 少 特征 点 数量 可 以 提高 运算 速度 。 读 
者 可 以 修改 运行 参数 和 数据 集 ， 看 看 它 在 各 种 情况 下 的 表现 。 


9.2.2 Wit 


本 节 ， 我 们 实现 了 一 个 简单 的 两 两 帧 间 的 视觉 里 程 计 ， 然 而 不 管 从 
速度 还 是 精度 上 来 说 ， 它 的 效果 都 不 理想 。 似 乎 这 种 看 似 简单 的 思路 并 
不 能 得 到 很 好 的 结果 。 我 们 来 考虑 一 下 有 哪些 可 能 的 原因 : 

1. 在 位 姿 估 计时 ， 我 们 使 用 了 RANSAC 求 出 的 PnP 解 。 由 于 RANSAC 
只 采用 少数 几 个 随机 点 来 计算 PnP， 虽 然 能 够 确定 inlier， 但 该 方法 容易 受 
噪声 影响 。 在 3D-2D 点 存在 噪声 的 情形 下 ， 我 们 要 用 RANSAC 的 解 作 为 初 
值 ， 再 用 非 线 性 优化 求 一 个 最 优 值 。 下 一 节 将 说 明 这 种 做 法 是 优 于 现在 
的 做 法 的 。 


2. 由 于 现在 的 VO 是 无 结构 的 ， 特 征 点 的 3D 位 置 被 当 作 真 值 来 估计 运 
动 。 但 实际 上 ，RGB-D 的 深度 图 必定 带 有 一 定 的 误差 特别 是 那些 深度 
过 近 或 过 远 的 地 方 。 并 且 ， 由 于 特征 点 往往 位 于 物体 的 边缘 处 ， 那 些 地 
方 的 深度 测量 值 通 常 是 不 准确 的 。 所 以 现在 的 做 法 不 够 精确 ， 我 们 需要 
把 特征 点 也 放 在 一 起 优化 。 


3. 只 考虑 参考 帧 /当前 帧 的 方式 ， 一 方面 使 得 位 次 估计 过 于 依赖 参考 
帧 。 如 果 人 参考 帧 质量 太 差 ， 比 如 ， 出 现 严重 遮挡 、 光 照 变 化 的 情况 下 ， 
跟踪 容易 丢失 。 并 且 ， 当 参考 帧 位 姿 估 计 不 准时 ， 还 会 出 现 明 显 的 漂移 
。 另 一 方面 ， 仅 使 用 两 帧 数据 显然 没有 充分 地 利用 所 有 的 信息 。 更 自然 
的 方式 是 比较 当前 帧 和 地 图 ， 而 不 是 比较 当前 帧 和 参考 帧 。 于 是 ， 我 们 
要 关心 如 何 把 当前 帧 与 地 图 进行 匹配 ， 以 及 如 何 优化 地 图 点 的 问题 。 


4. 由 于 输出 了 各 步骤 的 运行 时 间 ， 我 们 可 以 对 计算 量 有 一 个 大 概 的 了 
解 ( 表 9-1) o 


表 9-1 某 一 次 循环 的 各 步骤 用 时 


项 目 | 特征 提取 | 描述 子 计 算 | 特征 匹配 | PnP 求解 -他 总 计 
时 [a] 。 0.0011 i 0.0319 


可 以 看 到 ， 特 征 点 的 提取 和 匹配 占用 了 绝 大 多 数 的 计算 时 间 ， 而 看 
似 复杂 的 PnP 优化 ， 计 算 量 与 之 相 比 基本 可 以 忽略 。 因 此 ， 如 何 提高 特征 
提取 、 匹 配 算法 的 速度 ， 将 是 特征 点 方法 的 一 个 重要 的 主题 。 一 种 可 预 
见 的 方式 是 使 用 直接 法 / 光 流 ， 可 有 效 地 避 开 繁重 的 特征 计算 工作 。 本 书 
之 前 已 讨论 过 直接 法 和 交流 法 ， 读 者 不 妨 上 自行 尝试 一 下 。 


9.3 Bot: 优化 PnP 的 结果 


接 下来， 我 们 沿 着 之 前 的 内 容 ， 尝 试 一 些 改进 VO 的 方法 。 本 市 中 ， 
我 们 来 尝试 RANSAC PnP 加 上 和 迭代 优化 的 方式 估计 相机 位 资 ， 看 看 是 否 
对 前 一 节 的 效果 有 所 改进 。 

非 线 性 优化 问题 的 求解 ， 已 经 在 第 6 讲 和 第 7 讲 介 绍 过 了 。 由 于 本 节 
的 目标 是 估计 位 姿 而 非 结 构 ， 我 们 以 相机 位 姿 上 为 优化 变量 ， 通 过 最 小 化 
重 投影 误差 来 构建 优化 问题 。 3 我 们 自 定义 一 个 g2o 中 的 优化 
边 。 它 只 优化 一 个 位 姿 ， 因此 是 一 — TEI 


由 slambook/project/0.3/include/myslam/g20_types.h 


1 | class EdgeProjectXYZ2UVPose0nly: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g20 
::VertexSE3Expmap > 


a | a 
3 | public: 
4 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 


virtual void computeError() ; 


virtual void linearizeOplus(); 


virtual bool read( std::istreamk in ){} 


10 virtual bool write(std::ostreamk os) const {}; 
11 

12 Vector3d point_; 

13 Camera* camera_; 


把 三 维 点 和 相机 模型 放 入 它 的 成 员 变 量 中 ， 方 便 计 算 重 投 


雅 可 比 和 矩阵 : 
内 slambook/project/0.3/src/g20_types.cpp 


Ev 
me 


+o 


IR 


差 和 


{ 


_vertices[0] ); 


void EdgeProjectXYZ2UVPoseOnly: : computeError () 


_error = _measurement - camera_->camera2pixel ( 


pose->estimate() .map(point_) 


J$ 


g20::VertexSE3Expmap* pose 


J; 


void EdgeProjectXYZ2UVPose0nly::linearize0plus() 


g20::SE3Quat T ( pose->estimate() ); 


Vector3d xyz_trans 


T.map ( point_ ); 


double x = xyz_trans [0]; 


double y = xyz_trans[1]; 


double z 


double z_2 = z*Z; 


_jacobianOplusxXi 
_jacobianOplusXi 
_jacobian0plusXi 
_jacobian0plusXi 
_jacobian0plusXi 


_jacobian0plusXi 


_jacobianOplusXi 
_jacobian0plusXi 
_jacobian0OplusxXi 
_jacobian0plusXi 
_jacobian0plusXi 


_jacobianOplusXi 


~~ a IDNA INN 


DN 


xyz_trans [2] ; 


vwrwvyrvrvrwvrv wv 


~everwvrvrwv wv 


x*y/z_2 *camera_->fx_; 


- ( 1+ ( x*x/z_2 ) ) *camera_->fx_; 


y/z * camera_->fx_; 
-1./z * camera_->fx_; 
0; 


x/z_2 * camera_->fx_; 


( i+y*y/z_2 ) *camera_->fy_; 


-x*y/z_2 *camera_->fy_; 


-x/z *camera_->fy_; 
0; 
-1./z *camera_->fy_; 


y/z_2 *camera_->fy_; 


const g20::VertexSE3Expmap* pose = static_cast<const g20::VertexSE3Expmap*> ( 


static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] 


然后 ， 在 之 前 的 PoseEstimationPnP 函 数 中 ， 修 改 成 以 RANSACPnP 结 
果 为 初 值 ， 再 调用 g2o 进 行 优 化 的 形式 : 


四 slambook/project/0.3/src/visual_odometry.cpp 


1 | void Visual0dometry: :poseEstimationPnP() 

2 |{ 

3 

4 // using bundle adjustment to optimize the pose 

5 typedef g20::BlockSolver<g2o: :BlockSolverTraits<6,2>> Block; 

6 Block: :LinearSolverType* linearSolver = new g20::LinearSolverDense<Block: : 
PoseMatrixType>() ; 

7 Block* solver_ptr = new Block( linearSolver ); 

8 g20::OptimizationAlgorithmLevenberg* solver = new g2o:: 
OptimizationAlgorithmLevenberg ( solver_ptr ); 

9 g20::SparseOptimizer optimizer; 

10 optimizer.setAlgorithm ( solver ); 

11 

12 g20::VertexSE3Expmap* pose = new g20::VertexSE3Expmap(); 

13 pose->setId ( 0 ); 

14 pose->setEstimate ( g20::SE3Quat ( 

15 T_c_r_estimated_.rotation_matrix(), T_c_r_estimated_.translation() 

16 J ys 

17 optimizer.addVertex ( pose ); 

18 

19 // edges 

20 for ( int i=0; i<inliers.rows; i++ ) 

21 { 

2 int index = inliers.at<int>(i,0); 

23 // 3D -> 2D projection 


24 EdgeProjectXYZ2UVPose0nly* edge = new EdgeProjectXYZ2UVPoseOnly () ; 
25 edge->setId(i); 

26 edge->setVertex(0, pose); 

27 edge->camera_ = curr_->camera_.get(); 

28 edge->point_ = Vector3d( pts3d[index].x, pts3d[index].y, pts3d[index].z ); 
29 edge->setMeasurement( Vector2d(pts2d[index].x, pts2d[index].y) ); 
30 edge->setInformation( Eigen: :Matrix2d::Identity() ); 

31 optimizer.addEdge( edge ); 

32 } 

33 

34 optimizer.initializeOptimization() ; 

35 optimizer.optimize(10) ; 

36 

37 T_c_r_estimated_ = SE3 ( 

38 pose->estimate().rotation(), 

39 pose->estimate().translation() 

40 ) 


请 读者 运行 此 程序 ， 对 比 之 前 的 结果 。 你 将 发 现 估计 的 运动 明显 稳 

定 了 很 多 。 同时 ， 由 于 新 增 的 优化 仍 是 无 结构 的 ， 规 模 很 小 ， 对 计算 时 
间 的 影响 基本 可 以 忽略 不 计 。 整 体 的 视觉 里 程 计 计算 时 间 仍 在 30ms 左 
右 。 

讨论 

我 们 发现， 引入 和 迭代 优化 方法 之 后 ， 估 计 结 果 的 质量 比 纯粹 
RANSAC PnP 有 明显 的 提高 。 尽 管 我 们 依然 仅 使 用 两 两 帧 间 的 信息 ， 但 
得 到 的 运动 却 更 加 准确 、 平 稳 。 从 这 次 改进 中 我 们 看 到 了 优化 的 重要 
性 。 不 过 ， 0.3 版 本 的 VO 仍 过 两 两 顺 间 匹 配 的 局 限 性 影响 。 一 旦 视频 序列 
当中 有 冰 个 帧 丢失 ， 就 会 导致 后 续 的 帧 也 无 法 和 上 一 帧 匹配 。 下 面 ， 我 们 
把 地 图 引入 至 jvO 中 来 。 


9.4 改进 : 局 部 地 图 


本 节 我 们 将 VO 匹配 到 的 特征 点 放 到 地 图 中 ， 并 将 当前 帧 与 地 图 点 进 
行 匹 配 ， 计 算 位 姿 。 这 种 做 法 与 之 前 的 差异 如 图 9-6 所 示 。 


在 两 两 帧 间 比 较 时 ， 我 们 只 计算 参考 帧 与 当前 帧 之 间 的 特征 匹配 和 
运动 天 系 ， 在 计算 之 后 把 当前 帧 设 为 新 的 参考 帧 。 而 在 使 用 地 图 的 VO 
中 ， 每 个 帧 为 地 图 贡献 一 些 信息 ， 比 如 ， 添 加 新 的 特征 点 或 更 新 旧 特 征 
点 的 位 置 估计 。 地 图 中 的 特征 点 位 置 往往 是 使 用 世界 坐标 的 。 因 此 ， 当 
前 帧 到 来 时 ， 我 们 求 它 和 地 图 之 间 的 特征 匹配 与 运动 关系， 即 直 接 计算 了 
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图 9-6 “两 两 帧 VO 与 地 图 VO 工 作 原 理 的 差异 。 


这 样 做 的 好 处 是 ， 我 们 能 够 维护 一 个 不 断 更 新 的 地 图 。 只 要 地 图 是 
正确 的 ， 即 使 中 间 某 帧 出 了 差错 ， 仍 有 希望 求 出 之 后 那些 帧 的 正确 位 
置 。 请 注意 ， 我 们 现在 还 没有 详细 地 讨论 SLAM 的 建 图 问题 ， 所 以 这 里 
的 地 图 仅 是 一 个 临时 性 的 概念 ， 指 的 是 把 各 帧 特征 点 缓存 到 一 个 地 方 构 
成 的 特征 点 的 集合 。 


地 图 又 可 以 分 为 局 部 (Local) 地 图 和 全 局 (Global) 地 图 两 种 ， 由 
于 用 途 不 同 ， 往 往 分 开 讨 论 。 顾 名 思 义 ， 局 部 地 图 描述 了 附近 的 特征 点 
信息 一 一 我 们 只 保留 离 相 机 当前 位 置 较 近 的 特征 点 ， 而 把 远 的 或 视野 外 
的 特征 点 丢掉 。 这 些 特 征 点 是 用 来 和 当前 帧 匹配 来 求 相 机 位 置 的 ， 所 以 


我 们 希望 它 能 够 做 得 比较 快 。 另 一 方面 ， 全 局 地 图 则 记录 了 从 SLAM 运 行 
以 来 的 所 有 特征 点 。 显 然 它 的 规模 要 大 一 些 ， 主 要 用 来 表达 整个 环境 ， 
但 是 直接 在 全 局 地 图 上 定位 ， 对 计算 机 的 负担 就 太 大 了 。 它 主要 用 于 回 
环 检测 和 地 图 表达 。 


在 视觉 里 程 计 中 ， 我 们 更 关心 可 以 直接 用 于 定位 的 局 部 地 图 (如 果 
决心 要 用 地 图 的 话 ) 。 所 以 本 讲 我 们 来 维护 一 个 局 部 地 图 。 随 着 相机 运 
动 ， 我 们 往 地 图 里 添加 新 的 特征 点 。 我 们 仍然 要 提醒 读者 : 是 否 使 用 地 
图 取决 于 你 对 精度 一 效率 这 个 矛盾 的 把 握 。 我 们 完全 可 以 出 于 效率 的 考 
量 ， 使 用 两 两 无 结构 式 的 VO; 也 可 以 为 了 更 好 的 精度 ， 构 建 局 部 地 图 乃 
至 考虑 地 图 的 优化 。 

局 部 地 图 的 一 件 麻 烦 事 是 维护 它 的 规模 。 为 了 保证 实时 性 ， 我 们 需 
要 保证 地 图 规模 不 至 于 太 大 (否则 匹配 会 消耗 大 量 的 时 间 ) 。 此 外 ， 单 
个 帧 与 地 图 的 特征 匹配 存在 着 一 些 加 速 手段 ， 但 由 于 技术 上 比较 复杂 ， 
我 们 的 例 程 中 就 不 给 出 了 。 

现在 ， 来 实现 地 图 点 类 吧 。 我 们 稍 加 完善 之 前 没有 用 到 的 MapPoint 
XK, REEE RAA HE RRS 


四 slambook/project/0.4/include/myslam/mappoint.h 


class MapPoint 
{ 
public: 
typedef shared_ptr<MapPoint> Ptr; 
unsigned long idy SH LD 
static unsigned long factory_id_; // factory id 
bool good_; // wheter a good point 
Vector3d pos_; // Position in world 
Vector3d norm_; // Normal of viewing direction 


Mat descriptor_; // Descriptor for matching 


list<Frame*> observed_frames_; // key-frames that can observe this point 


int matched_times_; // being an inliner in pose estimation 


int visible_times_; // being visible in current frame 


MapPoint () ; 
MapPoint ( 
unsigned long id, 
const Vector3d& position, 
const Vector3d& norm, 
Frame* frame=nullptr, 
const Mat& descriptor=Mat () 
); 


inline cv::Point3f getPositionCV() const { 


return cv::Point3f( pos_(0,0), pos_(1,0), pos_(2,0) ); 


static MapPoint::Ptr createMapPoint(); 
static MapPoint::Ptr createMapPoint ( 
const Vector3d& pos_world, 
const Vector3d& norm_, 
const Mat& descriptor, 


Frame* frame 


主要 的 修改 在 VisualOdometry 类 上 。 由 于 工作 流程 的 改变 ， 我 们 修改 
了 它 的 几 个 主要 函数 ， 例 如 ， 每 次 循环 中 要 对 地 图 进行 增删 、 统 计 每 个 


地 图 点 被 观测 到 的 次 数 ， 等 等 上 。 这 些 事情 是 比较 琐碎 的 ， 所 以 我 们 还 
是 建议 读者 仔细 看 看 GitHub 上 提供 的 源 代 码 。 重 点 观察 以 下 几 项 : 


1. 在 提取 第 一 帧 的 特征 点 之 后 ， 将 第 一 帧 的 所 有 特征 点 全 部 放 入 地 图 


中 : 


1 | void VisualOdometry: :addKeyFrame() 


z |x, 


if ( map_->keyframes_.empty() ) 


{ 


} 


map_->insertKeyFrame ( curr_ ); 


// first key-frame, add all 3d points into map 


for ( size_t i=0; i<keypoints_curr_.size(); i++ ) 


{ 


ref_ 


double d = curr_->findDepth ( keypoints_curr_[i] ); 
if (d<0) 
continue; 
Vector3d p_world = ref_->camera_->pixel2world ( 
Vector2d ( keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y 
), curr_->T_c_w_, d 
); 
Vector3d n = p_world - ref_->getCamCenter() ; 
n.normalize(); 
MapPoint::Ptr map_point = MapPoint: :createMapPoint ( 
p_world, n, descriptors_curr_.row(i).clone(), curr_.get() 
); 


map_->insertMapPoint( map_point ); 


curr_; 


2.a2RB9 MA, EA OptimizeMap A š EATE. BSH RRA 
在 视野 内 的 点 ， 在 匹配 效 量 减少 时 添加 新 点 ， 等 等 。 


void Visual0dometry: : optimizeMap () 


{ 


// remove the hardly seen and no visible points 


for ( auto iter = map_->map_points_.begin(); iter != map_->map_points_. 
end(); ) 
{ 


if ( !curr_->isInFrame(iter->second->pos_) ) 
{ 
iter = map_->map_points_.erase(iter) ; 


continue; 


10 } 


11 float match_ratio = float(iter->second->matched_times_)/iter-> 


second->visible_times_; 


12 if ( match_ratio < map_point_erase_ratio_ ) 
13 { 

14 iter = map_->map_points_.erase(iter) ; 

15 continue; 

16 } 

17 double angle = getViewAngle( curr_, iter->second ); 
18 if ( angle > M_PI/6. ) 

19 { 

20 iter = map_->map_points_.erase(iter) ; 

21 continue; 

22 J 

23 if ( iter->second->good_ == false ) 

24 { 

25 // TODO try triangulate this map point 
26 } 

27 itert++; 

28 } 

29 

30 if ( match_2dkp_index_.size()<100 ) 

31 addMapPoints() ; 

32 if ( map_->map_points_.size() > 1000 ) 

33 { 

34 // TODO map is too large, remove some one 
35 map_point_erase_ratio_ += 0.05; 

36 } 

37 else 

38 map_point_erase_ratio_ = 0.1; 

39 cout<<"map points: "<<map_->map_points_.size()<<end1l; 


我 们 刻意 留 空 了 一 些 地 方 ， 请 感 兴 趣 的 读者 自行 完成 。 例 如 ， 你 可 


以 使 用 三 角 化 来 更 新 特征 点 的 世界 坐标 ， 或 者 考虑 更 好 地 动态 管 
规模 的 策略 。 这 些 问题 都 是 开放 性 的 。 


理 地 图 


sl eee 


视野 内 的 点 ) ， 然 后 将 它们 与 当前 帧 的 特征 描述 子 进行 匹配 。 


void Visual0dometry: :featureMatching() 


boost::timer timer; 
vector<cv::DMatch> matches; 
// select the candidates in map 


Mat desp_map; 


7 vector<MapPoint::Ptr> candidate; 

8 for ( auto& allpoints: map_->map_points_ ) 

9 { 

10 MapPoint::Ptr& p = allpoints.second; 

11 // check if p in curr frame image 

12 if ( curr_->isInFrame(p->pos_) ) 

13 { 

14 // add to candidate 

15 p->visible_times_++; 

16 candidate.push_back( p ); 

17 desp_map.push_back( p->descriptor_ ); 

18 } 

19 } 

20 

21 matcher_flann_.match ( desp_map, descriptors_curr_, matches is 
2 // select the best matches 

23 float min_dis = std::min_element ( 

24 matches.begin(), matches.end(), 

25 [] ( const cv::DMatch& mi, const cv::DMatch& m2 ) 
26 { 

27 return mi.distance < m2.distance; 

28 } )->distance; 

29 

30 match_3dpts_.clear() ; 

31 match_2dkp_index_.clear() ; 

32 for ( cv::DMatch& m : matches ) 

33 { 

34 if ( m.distance < max<float> ( min_dis*match_ratio_, 30.0 ) ) 
35 { 

36 match_3dpts_.push_back( candidate[m.queryIdx] ); 
37 match_2dkp_index_.push_back( m.trainIdx ); 

38 } 

39 } 

40 cout<<"good matches: "<<match_3dpts_.size() <<endl; 

41 cout<<"match cost time: "<<timer.elapsed() <<endl; 

a |} 


除了 现 有 的 地 图 之 外 ， 我 们 还 引入 了 “关键 帧 ” (Key-frame) 的 概 
念 。 关 键 帧 在 许多 视觉 SLAM 中 都 会 用 到 ， 不 过 这 个 概念 主要 是 给 后 端 用 
的 ， 所 以 我 们 在 后 面 几 讲 再 讨论 对 关键 帧 的 详细 处 理 。 在 实践 中 ， 我 们 


肯定 不 希望 对 每 个 图 像 都 做 详细 的 优化 和 回环 检测 ， 那 样 毕 竟 太 耗费 资 
源 。 至 少 相 机 搁 在 原 地 不 动 时 ， 我 们 不 希望 整个 模型 (地 图 也 好 、 轨 迹 
也 好 ) 变 得 越 来 越 大 。 因 此 ， 后 端 优化 的 主要 对 象 就 是 关键 帧 。 


关键 帧 是 相机 运动 过 程 当中 某 几 个 特殊 的 帧 ， 这 里 “特殊 ”的 意义 是 
可 以 由 我 们 目 己 指定 的 。 党 见 的 做 法 时 ， 每 当 相 机 运动 经 过 一 定 间隔 ， 
束 取 一 个 新 的 关键 帧 并 保存 起 来 中 。 这 些 关 键 帧 的 位 姿 将 被 仔细 优化 ， 
而 位 于 两 个 天 键 帧 之 间 的 那些 东西 ， 除 了 对 地 图 贡献 一 些 地 图 点 外 ， 融 
被 理所当然 地 忽略 掉 了 。 

本 节 的 实现 也 会 提取 一 些 关 键 帧 ， 为 后 端的 优化 做 一 些 数 据 上 的 准 
备 。 现 在 ， 读 者 可 以 编译 这 个 工程 ， 看 看 它 的 运行 结果 。 本 节 的 例 程 会 
把 局 部 地 图 的 点 投影 到 图 像 平 面 并 显示 出 来 。 如 果 位 姿 估 计 正 确 ， 它 们 
看 起 来 应 该 像 是 固定 在 空间 中 一 样 。 反 之 ， 如 果 你 感觉 到 某 个 特征 点 不 
自然 地 运动 ， 那 可 能 是 相机 位 次 估计 不 够 准确 ， 或 特征 点 的 位 置 不 够 准 
确 。 

我 们 在 0.4 版 没有 提供 对 地 图 的 优化 ， 建 议 读者 自行 尝试 一 下 。 用 到 
的 原理 主要 是 最 小 二 乘 和 三 角 化 ， 在 前 两 讲 已 介绍 过 ， 不 会 太 困难 。 
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图 9-7 ”0.4 版 YO 的 运行 截图 ， 在 两 个 不 同时 刻 标记 了 路 标 投影 点 。 
9.5 小结 


作为 实践 ， 本 讲 市 领 读 者 从 零 开始 实现 了 一 个 简单 的 视觉 里 程 计 ， 
为 的 是 让 读者 对 前 面 两 讲 介绍 的 算法 有 一 个 经 验 上 的 认识 。 如 果 没 有 本 
讲 ， 你 很 难 杀 身体 会 到 例如 “特征 点 的 VO 大 约 能 够 实时 处 理 多 少 个 ORB 
特征 点 ”这 样 的 问题 。 我 们 看 到 ， 视 觉 里 程 计 能 够 估算 局 部 时 间 内 的 相 
机 运动 及 特征 点 的 位 置 ， 但 是 这 种 局 部 的 方式 有 了 明显 的 缺点 : 


1. 容 易 丢 失 。 一 旦 丢失 ， 我 们 要 么 “等 待 相机 转 回来 ”( 保 存 人 参考 帧 并 
与 新 的 帧 比较 ) ， 要 么 重 置 整个 VO 以 跟踪 新 的 图 像 数 据 。 

2. 轨 迹 漂移 。 主 要 原因 是 每 次 估计 的 误差 会 累积 至 下 一 次 估计 ， 导 致 
长 时 间 轨 迹 不 准确 。 大 一 点 的 局 部 地 图 可 以 缓解 这 种 现象 ， 但 它 始终 是 
存在 的 。 

值得 一 提 的 是 ， 如 果 只 关心 短 时 间 内 的 运动 ， 或 者 VO 的 精度 已 经 满 
足 应 用 需求 ， 那 么 有 了 时候 你 可 能 需要 的 仅仅 就 是 一 个 视觉 里 程 计 ， 而 不 
用 完全 的 SLAM。 比 如 ， 某 些 无 人 机 控制 或 AR 游戏 的 应 用 中 ， 用 户 并 不 
需要 一 个 全 局 一 致 的 地 图 ， 那 么 轻便 的 VO 可 能 是 更 好 的 选择 。 不 过 ， 本 
书 的 目标 是 介绍 整个 LAM， 所 以 我 们 还 要 走 得 更 远 一 些 ， 看 看 后 端 和 回 
环 检测 是 如 何 工作 的 。 

习题 

1. 本 书 使 用 的 C++ 技巧 你 都 看 懂 了 吗 ?” 如 果 有 不 明白 的 地 方 ， 使 用 搜 


索引 擎 补习 相关 的 知识 ， 包 括 : 基于 范围 的 for 循 环 、lambda 表 达 式 、 智 
能 指针 、 设 计 模 式 中 的 单 例 模式 ， 等 等 。 


2. 在 0.3 版 或 0.4 版 的 基础 上 ， 添 加 对 地 图 进行 优化 的 代码 。 或 者 ， 也 
可 以 根据 PnP 结 果 做 一 下 三 角 化 ， 消 除 RGB-D 深 度 值 的 误差 。 


3. 观 察 本 讲 代码 是 如 何 处 理 误 匹 配 的 。 什 么 是 RANSAC? 阅读 文献 
[61] 或 搜索 相关 资料 进行 了 解 。 
[1] 左下 是 笔者 的 练习 作品 。 右 下 来 自 Epicwork 团 队 作品 : 《圆明园 》。 
[2] 当然 ， 从 C++ 设计 角度 来 说 ， 保 留 之 前 的 方式 并 使 用 继承 会 更 有 效 地 复 用 现 有 代码 。 
[3] 这 在 李 代 数 上 很 容易 实现 ， 请 想 想 怎么 实现 。 


[4] 当然 这 取决 于 你 的 机 器 的 性 能 。 
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主要 目标 

1. 理 解 后 端的 概念 。 

2. 理 解 以 EKF 为 代表 的 滤 疲 器 后 端 工作 原理 。 

3. 理 解 非 线性 优化 的 后 端 ， 明 白 稀 牙 性 是 如 何 利 用 的 。 

4. 使 用 g2o 和 Ceres 实 际 操作 后 端 优化 。 

本 讲 开始 ， 我 们 转 入 SLAM 系 统 的 另 一 个 重要 模块 : 后 端 优化 。 


我 们 看 到 ， 前 端 视觉 里 程 计 能 给 出 一 个 短 时 间 内 的 轨迹 和 地 图 ， 但 
由 于 不 可 避免 的 误差 累积 ， 这 个 地 图 在 长 时 间 内 是 不 准确 的 。 所 以 ， 在 
视觉 里 程 计 的 基础 上 ， 我 们 还 希望 构建 一 个 尺度 、 规 模 更 大 的 优化 间 
题 ， 以 考虑 长 时 间 内 的 最 优 轨 迹 和 地 图 。 不 过 ， 考 虑 到 精度 与 性 能 的 平 
衡 ， 实 际 当中 存在 着 许多 不 同 的 做 法 。 
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10.1 概述 
10.1.1 ”状态 估计 的 概率 解释 


第 2 讲 中 提 到 ， 视 觉 里 程 计 只 有 短暂 的 记忆 ， 而 我 们 希望 整个 运动 轨 
迹 在 较 长 时 间 内 都 能 保持 最 优 的 状态 。 我 们 可 能 会 用 最 新 的 知识 ， 更 新 
较 久 远 的 状态 一 一 站 在 “久远 的 状态 ”的 角度 上 看 ,仿佛 是 未 来 的 信息 告 
诉 它 “ 你 应 该 在 哪里 ”"。 所 以 ， 在 后 端 优化 中 ， 我 们 通常 考虑 一 段 更 长 时 
间 内 (或 所 有 时 间 内 ) 的 状态 估计 问题 ， 而 且 不 仅 使 用 过 去 的 信息 更 新 
自己 的 状态 ， 也 会 用 未 来 的 信息 来 更 新 自己 ， 这 种 处 理 方式 不 妨 称 为 “ 批 
=AY” (Batch) 。 否 则 ， 如 果 当 前 的 状态 只 由 过 去 的 时 刻 决定 ， 甚 至 只 由 
前 一 个 时 刻 决定 ， 那 不 妨 称 为 “渐进 的 ”(Incremental) o 

我 们 已 经 知道 SLAM 过 程 可 以 由 运动 方程 和 观测 方程 来 描述 。 那 么 ， 
假设 在 t =0 到 t =N 的 时 间 内 ， 有 位 姿 x ,到 x, ， 并 且 有 路 标 y , …,yw。 按照 
之 前 的 写法 ， 运 动 和 观测 方程 为 


CS Tp Uk + Wk 
| k = f (Tk-1, Uk) + We en N,j=1,..., M. (10.1) 


Zk j = h (Yj, Lk) + Vk,j 


注意 以 下 几 点 : 

1. 观 测 方程 中 ， 只 有 当 x, 看 到 了 y 时 ， 才 会 产生 观测 数据 ， 否 则 就 没 
有 。 事 实 上 ， 在 一 个 位 置 通常 只 能 看 到 一 小 部 分 路 标 。 而 且 ， 由 于 视觉 
SLAM 特 征 点 数量 众多 ， 所 以 实际 当中 观测 方程 的 数量 会 远 远大 于 运动 方 
程 。 

2. 我 们 可 能 没有 测量 运动 的 装置 ， 所 以 也 可 能 没有 运动 方程 。 在 这 个 
情况 下 ， 有 若干 种 处 理 方 式 : 认为 确实 没有 运动 方程 ， 或 假设 相机 不 
动 ， 或 假设 相机 匀速 运动 。 这 几 种 方式 都 是 可 行 的 。 在 没有 运动 方程 的 
情况 下 ， 整 个 优化 问题 就 只 由 许多 个 观测 方程 组 成 。 这 就 非常 类 似 于 SfM 
(Structure from Motion) 问题 ， 相 当 于 我 们 通过 一 组 图 像 来 恢复 运动 和 
结构 。 与 SfIM 中 不 同 的 是 ，SLAM 中 的 图 像 有 时 间 上 的 先后 顺序 ， 而 Sf{M 
中 人 允许 使 用 完全 无 关 的 图 像 。 


我 们 知道 每 个 方程 都 受 噪声 影响 ， 所 以 要 把 这 里 的 位 次 x 和 路 标 y 看 
成 服从 某 种 概率 分 布 的 随机 变量 ， 而 不 是 单独 的 一 个 数 。 因 此 ， 我 们 关 
心 的 问题 就 变 成 : 当 我 拥有 某 些 运动 数据 u 和 观测 数据 z 时 ， 如 何 来 确定 
状态 量 x,y 的 分 布 ? 进而 ， 如 果 得 到 了 新 时 刻 的 数据 ， 那 么 它们 的 分 布 又 
将 发 生 怎样 的 变化 ? 在 比较 常见 且 合 理 的 情况 下 ， 我 们 假设 状态 量 和 噪 
声 项 服从 高 斯 分 布 一 一 这 意味 着 在 程序 中 只 需要 储存 它们 的 均值 和 协 方 
差 矩 阵 即 可 。 均 值 可 看 作 是 对 变量 最 优 值 的 估计 ， 而 协 方差 矩阵 则 度量 
了 它 的 不 确定 性 。 那 么 ， 问 题 就 转变 为 : 当 存 在 一 些 运动 数据 和 观测 数 
据 时 ， 我 们 如 何 去 估 计 状 态 量 的 高 斯 分 布 ? 


我 们 依然 设身处地 地 扮演 一 下 小 萝卜 。 只 有 运动 方程 时 ， 相 当 于 我 
们 蒙 着 眼睛 在 一 个 未 知 的 地 方 走路 。 尽 管 我 们 知道 自己 每 一 步 走 了 多 
远 ， 但 是 随 着 时 间 增 长 ， 我 们 将 对 自己 的 位 置 越 来 越 不 确定 一 一 内 心 也 
就 越 不 安 。 这 说 明 在 输入 数据 受 噪声 影响 时 ， 我 们 对 位 置 方 差 的 估计 将 
越 来 越 大 。 但 是 ， 当 我 们 睁 开眼 睛 时 ， 由 于 能 够 不 断 地 观测 到 外 部 场 
景 ， 使 得 位 置 估计 的 不 确定 性 变 小 ， 我 们 就 会 越 来 越 自信 。 如 果 用 椭圆 
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走路 的 感觉 。 以 图 10-1 为 例 ， 读 者 可 以 想象 ， 当 没有 观测 数据 时 ， 这 个 圆 
会 随 着 运动 越 来 越 大 ; 而 如 果 有 正确 的 观测 ， 圆 就 会 缩小 至 一 定 的 大 
小 ， 保 持 稳定 。 


图 10-1 不 确定 性 的 直观 描述 。 左 侧 : 只 有 运动 方程 时 ， 由 于 下 一 时 刻 的 位 姿 是 在 上 一 时 刻 
基础 上 添加 了 噪声 ， 所 以 不 确定 性 越 来 越 大 。 右 侧 : 存在 路 标点 时 ， 不 确定 性 会 明显 减 
小 。 不 过 请 注意 ， 这 只 是 一 个 直观 的 示意 图 ， 并 非 实际 数据 。 


上 面 的 过 程 以 比喻 的 形式 解释 了 状态 估计 中 的 问题 ， 下 面 我 们 要 以 


定量 的 方式 来 看 待 它 。 在 第 6 讲 中 ， 我 们 介绍 了 最 大 似 然 估 计 ， 提 到 把 状 
态 估计 转换 为 最 小 二 乘 的 做 法 。 在 本 讲 我 们 要 更 仔细 地 讨论 这 个 问题 。 


首先 ， 由 于 位 姿 和 路 标点 都 是 待 估计 的 变量 ， 我 们 改变 一 下 记号 ， 令 x 

为 k 时刻 的 所 有 未 知 量 。 它 包含 了 当前 时 刻 的 相机 位 姿 与 m 个 路 标点 。 在 

这 种 记号 的 意义 下 虽然 与 之 前 稍 有 不 同 ， 但 含义 是 清楚 的 ) ， 写 成 : 
Tk = {Lk Y1,--->Ym}- (10.2) 


同时 ， 把 K 时 刻 的 所 有 观测 记 作 z 。 于 是 ， 运 动 方程 与 观测 方程 的 形 


式 可 写 得 更 加 简洁 。 这 里 不 会 出 现 y ， 但 我 们 心里 要 明日 这 时 x 中 已 经 包 
含 了 之 前 的 y : 


Lk = J (Lk-1, Uk) + Wk 
人 Ts (10.3) 


Zk = h (£k) + Vk 
现在 考虑 第 k 时 刻 的 情况 。 我 们 希望 用 过 去 0 到 k 中 的 数据 来 估计 现 
在 的 状态 分 布 : 


P(e |; U1:k, Z1:k)-. (10.4) 


下 标 0:k 表示 从 0 时 刻 到 k 时 刻 的 所 有 数据 。 请 注意 ，z 表示 所 有 在 k 
时 刻 的 观测 数据 ， 它 可 能 不 止 一 个 ， 只 是 这 种 记 法 更 加 方便 。 


下 面 我 们 来 看 如 何 对 状态 进行 估计 。 按 照 贝 叶 斯 法 则 ， 把 z, Sx, 交换 
位 置 ， 有 : 
P (£k|£0, U1:k, Z1:k) X P (zk|£k) 已 (ZEZoy U1:k, 21:k-1) - (10.5) 


读者 应 该 不 会 感到 陌生 。 这 里 第 一 项 称 为 似 然 ， 第 二 项 称 为 先 验 。 
似 然 由 观测 方程 给 十， 而 先 验 部 分 ， 我 们 要 明日 当前 状态 x, 是 基于 过 去 


所 有 的 状态 估计 得 来 的 。 至 少 ， 它 会 受 x | 影响， 于 是 按照 x NAAR 
件 概率 展开 : 


P (@|%05t1:k5 21:k-1) = | Pleven a, Zo, u zara) P (whileor tisk ia) dk 


(10.6) 


如 果 考 虑 更 久之 前 的 状态 ， 也 可 以 继续 对 此 式 进 行 展 开 ， 但 现在 我 
们 只 关心 k 时 刻 和 k- 1 时 刻 的 情况 。 至 此 ， 我 们 给 出 了 贝 叶 斯 估计 ， 虽 然 
上 了 式 还 没有 具体 的 概率 分 布 形式 ， 所 以 还 没 法 实际 地 操作 它 。 对 这 一 步 
的 后 续 处 理 ， 方 法 上 产生 了 一 些 分 上 层 。 大 体 上 讲 ， 和 存在 若干 种 选择 : 其 
一 是 假设 马尔 可 夫 性 ， 简 单 的 一 阶 马 氏 性 认为 ，k 时 刻 状态 只 与 k- 1 时 刻 
状态 有 关 ， 而 与 再 之 前 的 无 关 。 如 果 做 出 这 样 的 假设 ， 我 们 就 会 得 到 以 
扩展 卡尔 曼 滤 波 (EKF) 为 代表 的 滤波 器 方法 。 在 滤波 方法 中 ， 我 们 会 
从 某 时 刻 的 状态 估计 ， 推 导 到 下 一 个 时 刻 。 另 外 一 种 方法 是 依然 考虑 k 时 
刻 状态 与 之 前 所 有 状态 的 关系 ， 此 时 将 得 到 非 线 性 优化 为 主体 的 优化 框 
染 。 非 线性 优化 的 基本 知识 已 在 前 文 介绍 过 。 目 前 视觉 SLAM 的 主流 为 非 
线性 优化 方法 。 不 过 ,为 了 让 本 书 更 全 面 ， 我 们 要 先 介绍 一 下 卡尔 曼 滤 
波 器 和 EKF 的 原理 。 


10.1.2 ”线性 系统 和 KF 


我 们 首先 来 看 滤波 器 模型 。 当 我 们 假设 了 马尔 可 夫 性 ， 从 数学 角度 
会 发 生 哪些 变化 呢 ? 首先 ， 当 前 时 刻 状 态 只 和 上 一 个 时 刻 有 关 ， 式 (10.6) 
中 右 侧 第 一 部 分 可 进一步 简化 : 


P (Ek|Ek—1, £0, Uk, Ft) = P (les Uk). (10.7) 


这 里 ， 由 于 k 时 刻 状 态 与 k- 1 之 前 的 无 天 ， 所 以 就 简化 成 只 与 % Mu 
有 天 的 形式 ， 与 K 时 刻 的 运动 方程 对 应 。 第 二 部 分 可 简化 为 


P (ZK 1Zo U1:k; 21:k-1) = P (@e-1|Vo, Uk 1, 21:k—1) - (10.8) 


这 是 考虑 到 k 时 刻 的 输入 量 u 与 k- 1 时 刻 的 状态 无 关 ， 所 以 我 们 把 ul 
拿 掉 。 可 以 看 到 ， 这 一 项 实际 上 是 k- 1 时 刻 的 状态 分 布 。 于 是 ， 这 一 系列 
方程 说 明 ， 我 们 实际 在 做 的 是 “如 何 把 k- 1 时 刻 的 状态 分 布 推导 至 k 时 刻 ” 
这 样 一 件 事 。 也 就 是 说 ， 在 程序 运行 期 间 ， 我 们 只 要 维护 一 个 状态 量 ， 
对 它 不 断 地 进行 迭代 和 更 新 即 可 。 进 一 步 ， 如 果 假 设 状态 量 服从 高 斯 分 
布 ， 那 么 我 们 只 需 考 虑 维护 状态 量 的 均值 和 协 方差 即 可 。 


我 们 从 形式 最 简单 的 线性 高 斯 系统 开始 ， 最 后 会 得 到 卡尔 曼 滤 六 
器 。 线 性 高 斯 系统 是 说 ， 运 动 方程 和 观测 方程 可 以 由 线性 方程 来 描述 : 


Lk = ÅkLk—1 + Uk + we 
| E E =e N. (10.9) 


Zk = Crap + VE 


并 假设 所 有 的 状态 和 噪声 均 满足 高 斯 分 布 。 记 这 里 的 噪声 服从 零 均 
值 高 斯 分 布 : 
Wk ~ N (0, R). Uk ~ N(0, Q). (10.10) 
为 了 简洁 ， 笔 者 省 略 了 R 和 Q 的 下 标 。 现 在 ， 利 用 马尔 可 夫 性 ， 假 设 


我 们 知道 了 k- 1 时 刻 的 后 验 (在 k- 1 时 刻 看 来 ) 状态 估计 YX:-1 及 其 协 方差 
Poo, IVE BARR 时 刻 的 输入 和 观测 数据 ， 确 定 x, 的 后 验 分 布 。 为 区 分 
推导 中 的 先 验 和 后 验 ， 我 们 在 记号 上 做 一 点 区 别 : AREF 表示 后 
验 ， 以 横 线 & 表示 移 验 分 布 ， 请 谈 者 不 要 混 痛 。 

卡尔 曼 滤波 器 的 第 一 步 ， 通 过 运动 方程 确定 x, 的 先 验 分 布 。 根 据 高 
斯 分 布 的 性 质 ， 显 然 有 : 


P (£k|£0, U1-k, 21:k-1) =N (Aner + up, AP, AT + R) 。 (10.11) 

这 一 步 称 为 预测 ， 原 理 见 附录 A.3。 它 显示 了 如 何 从 上 一 个 时 刻 的 状 
态 ， 根 据 输入 信息 〈 但 是 有 噪声 ) 推断 当前 时 刻 的 状态 分 布 。 这 个 分 布 
也 就 是 先 验 。 记 : 


Tk = ÅkÊk—1ı + Uk, P, = AkP, 1AT + R. (10.12) 


这 非常 目 然 。 另 一 方面 ， 由 观测 方程 ， 我 们 可 以 计算 在 某 个 状态 下 
应 该 产生 怎样 的 观测 数据 : 


P (zk|£k) = N (Cker, R). (10.13) 


为 了 得 到 后 验 概率 ， 我 们 想 要 计算 它们 的 乘积 ， 也 就 是 由 式 (10.5) 给 
出 的 贝 叶 斯 公式 。 然 而 ， 虽 然 我 们 知道 最 后 会 得 到 一 个 关于 x 的 高 斯 分 


布 ， 但 计算 上 是 有 一 点 点 麻烦 的 ， 我 们 先 把 结果 设 为 x N Cx, Pi), BB 
Az: 
N (êp, Pr) = N (Cker, R) . N(x, Px). (10.14) 
这 里 我 们 稍微 用 点 讨 巧 的 方法 。 既 然 我 们 已 经 知道 等 式 两 侧 都 是 高 
斯 分 布 ， 那 就 只 需 比 较 指数 部 分 即 可 ， 而 无 须 理 会 高 斯 分 布 前 面 的 因子 
部 分 。 指 数 部 分 很 像 是 一 个 二 次 型 的 配方 ， 我 们 来 推导 一 下 。 首 先 把 指 
数 部 分 展开 ， 有 中 : 


(£k — ĉr) P-! (£k 一 Lk) = (Zk 一 Crx) Ro! (Zk 一 CCD) 十 (ZK 一 Z) P-t (£k — Tp) : 
(10.15) 


为 了 求 左 侧 的 2* FP: ， 我 们 把 两 边 展开 ， 并 比较 x, 的 二 次 和 一 次 系 
数 。 对 于 二 次 系数 ， 有 : 


Pot =Ci RC, +P". (10.16) 


该 式 给 出 了 协 方 差 的 计算 过 程 。 为 了 便于 后 面 列 写 式 子 ， 定 义 一 个 
中 间 变 量 : 


K=P,CiR". (10.17) 


MIBILEM, EROOIODWEAK RÊ, B: 


I = ÊC] RC, + P,.P,! = KC, + iP. (10.18) 
FEA": 
PÊ, = (I — KC,)P,. (10.19) 
然后 再 比较 一 次 项 的 系数 ， 有 : 
一 2 你 万- ‘x = —22/ R'Cy ap 一 22i P ‘ak. (10.20) 


整理 〈 取 系数 并 转 置 ) 得 : 


Ê tp = CL R12, + Pra. (10.21) 
两 侧 乘 以 P: 并 代入 式 (10.17)， 得 : 


êk = PLCI R ' zp + Ê, P 'Ep (10.22) 
= Kz + (I — KC) Tk = k + K (Zk — Chk). (10.23) 
于 是 我 们 又 得 到 了 后 验 均 值 的 表达 。 总 而 言 之 ， 上 面 的 两 个 步骤 可 
以 归纳 为 “预测 ” (Predict) 和 “更 新 ”(Update) 两 个 步骤 : 
1. 预 测 : 


Bp = AkÊêk-1 + ur, P, = Ap Py-1A,+ RB. (10.24) 
2. 更 新 : 先 计 算 K ， 它 又 称 为 卡尔 曼 增 益 。 
K = Pic. (Cr PCr 十 Ry). (10.25) 


然后 计算 后 验 概率 的 分 布 。 
Êk = k + K (Zk 一 CkĒk) 
让 三 全 一 二 区 9 


至 此 ， 我 们 推导 了 经 典 的 卡尔 曼 滤 波 器 的 整个 过 程 。 事 实 上 ， 卡 尔 
曼 滤 疲 器 有 若干 种 推导 方式 ， 而 我 们 使 用 的 是 从 概率 角度 出 发 的 最 大 后 
验 概率 估计 的 形式 。 我 们 看 到 ， 在 线性 高 斯 系统 中 ， 卡 尔 曼 滤 波 器 构成 
了 该 系统 中 的 最 大 后 验 概率 估计 。 而 且 ， 由 于 高 斯 分 布 经 过 线性 变换 后 
仍 服 从 高 斯 分 布 ， 所 以 整个 过 程 中 我 们 没有 进行 任何 的 近似 。 可 以 说 ， 
卡尔 曼 滤 波 器 构成 了 线性 系统 的 最 优 无 偏 估计 。 


10.1.3” 非 线性 系统 和 EKF 


在 理解 了 卡尔 曼 滤 疲 之 后 ， 我 们 必须 要 澄清 一 点 : SLAM 中 的 运动 方 
程 和 观测 方程 通常 是 非 线 性 消 数 ， 尤 其 是 视觉 SLAM 中 的 相机 模型 ， 需 要 
使 用 相机 内 参 模型 及 李 代 数 表示 的 位 姿 ， 更 不 可 能 是 一 个 线性 系统 。 一 
个 高 斯 分 布 ， 经 过 非 线 性 变换 后 ， 往 往 不 再 是 高 斯 分 布 ， 所 以 在 非 线性 
系统 中 ， 我 们 必须 取 一 定 的 近似 ， 将 一 个 非 高 斯 分 布 近似 成 高 斯 分 布 。 


(10.26) 


我 们 希望 把 卡尔 曼 滤波 器 的 结果 拓展 到 非 线 性 系统 中 ， 称 为 扩展 卡 
尔 曼 滤波 器 (Extended Kalman Filter, EKF) 。 通 常 的 做 法 是 ， 在 某 个 点 
附近 考虑 运动 方程 及 观测 方程 的 一 阶 泰勒 展开 ， 只 保留 一 阶 项 ， 即 线性 
的 部 分 ， 然 后 按照 线性 系统 进行 推导 。 仿 上 -1 时 刻 的 均值 与 协 方差 矩阵 为 
$i_1, 记 _1。 在 Kk 时刻， 我们 把 运动 方程 和 观测 方程 在 %_1, 记 1 处 进行 线性 
化 (相当 于 一 阶 泰勒 展开 ) ， 有 : 


of 


Tk X f (k1, Uk) 十 T (£k-1 — Êk—1) + Wk. (10.27) 

记 这 里 的 偏 导 数 为 
_ f 

Ser i (10.28) 

同样 ， 对 于 观测 方程 ， 亦 有 : 
h 
zk Th (Bie) a 网 (Lk — Êk) + Ng. (10.29) 

记 这 里 的 偏 导 数 为 
Oh 

n S (10.30) 


那么 ， 在 预测 步骤 中 ， 根 据 运动 方程 有 : 
P (x~|Zo, U1:k, 20:k—1) = N(f (ĉk—1, Uk) R FÊ, F" 十 Rx). (10.31) 


这 些 推导 和 卡尔 曼 滤 波 是 十 分 相似 的 。 为 方便 表述 ， 记 这 里 的 先 验 
和 协 方差 的 均值 为 


Bk =f (k—1, Uk) ; P, = FP,F" + Rp. (10.32) 


然后 ， 考 虑 在 观测 中 我 们 有 : 


P (zk\|£k) = N(h (Tk) +H (£k E Tk) 3 Qk). (10.33) 


最 后 ， 根 据 最 开始 的 贝 叶 斯 展开 式 ， 可 以 推导 出 x 的 后 验 概率 形 
式 。 我 们 略 去 中 间 的 推导 过 程 ， 只 介绍 其 结果 。 读 者 可 以 仿照 卡尔 曼 滤 
疲 器 的 方式 ， 推 导 EKF 的 预测 与 更 新 方程 。 简 而 言 之 ， 我 们 会 先 定 义 一 
个 卡尔 最 增益 K,, : 


K, = P,H"(HP,H"™ + Qy) . (10.34) 
在 卡尔 曼 增 益 的 基础 上 ， 后 验 概率 的 形式 为 
Êk = Tk Kr (zk —h(&,)), Py = (I — KH) P,. (10.35) 


卡尔 曼 滤波 器 给 出 了 在 线性 化 之 后 状态 变量 分 布 的 变化 过 程 。 在 线 

性 系统 和 高 斯 噪声 下 ， 卡 尔 曼 滤波 器 给 出 了 无 偏 最 优 估 计 。 而 在 SLAM 这 

种 非 线 性 的 情况 下 ， 它 给 出 了 单 次 线性 近似 下 的 最 大 后 验 估计 
(MAP) 。 


10.1.4 EKF 的 讨论 


EKF 以 形式 简洁 、 应 用 广泛 著称 。 当 想 要 在 某 段 时 间 内 估计 某 个 不 
确定 量 时 ， 我 们 首先 想到 的 就 是 EKF。 在 早期 的 SLAM 中 ，EKF 占 据 了 很 
长 一 段 时 间 的 主导 地 位 ， 研 究 者 们 讨论 了 各 种 各 样 的 滤波 器 在 SLAM 中 的 
MA, WIF (信息 滤波 器 ) n, IKF (Iterated KF) 、 UKF” 

(Unscented KF) PMF VERA 97] | SWE (Sliding Window Filter) 1° 
， 等 等 (7B ， 或 者 用 分 治 法 等 思路 改进 EKF 的 效率 3H 。 直 至 今日 ， 尽 
管 我 们 认识 到 非 线 性 优化 比 滤波 器 占有 明显 的 优势 ， 但 是 在 计算 资源 受 
限 ， 或 待 估计 量 比较 简单 的 场合 ，EKF 仍 不 失 为 一 种 有 效 的 方式 。 

EKF 有 哪些 局 限 呢 ? 


1. 首 先 ， 滤 波 器 方法 在 一 定 程度 上 假设 了 马尔 可 夫 性 ， 也 就 是 k 时 刻 
的 状态 只 与 

k- 1 时 刻 相关 ， 而 与 上 1 之 前 的 状态 和 观测 都 无 天 《或 者 和 前 几 个 有 
限时 刻 的 状态 相关 ) 。 这 有 点 像 是 在 视觉 里 程 计 中 只 考虑 相 邻 两 帧 关系 
一 样 。 如 果 当 前 帧 确实 与 很 久之 前 的 数据 有 关 (例如 回环 ) ， 那 么 滤波 
器 就 会 难以 处 理 。 


而 非 线 性 优化 方法 则 倾向 于 使 用 所 有 的 历史 数据 。 它 不 光 考 虑 邻近 
时 刻 的 特征 点 与 轨迹 关系 ， 更 会 把 很 久之 前 的 状态 也 考虑 进来 ， 称 为 全 
体 时 间 上 的 SLAM (Full-SLAM) 。 在 这 种 意义 下 ， 非 线性 优化 方法 使 用 
了 更 多 信息 ， 当 然 也 需要 更 多 的 计算 。 


2. 与 第 6 讲 介绍 的 优化 方法 相 比 ，EKF 滤 波 器 仅 在 xx, , 处 做 了 一 次 线 


性 化 ， 然 后 就 直接 根据 这 次 线性 化 结果 ， 把 后 验 概率 给 算 了 出 来 。 这 相 
当 于 在 说 ， 我 们 认为 该 点 处 的 线性 化 近似 在 后 验 概率 处 仍然 是 有 效 的 。 
而 实际 上 ， 当 我 们 离开 工作 点 较 远 时 ， 一 阶 泰勒 展开 并 不 一 定 能 够 近似 
整个 水 数 ， 这 取决 于 运动 模型 和 观测 模型 的 非 线 性 情况 。 如 果 它 们 有 强 
烈 的 非 线性 ， 那 么 线性 近似 融 只 在 很 小 范围 内 成 立 ， 不 能 认为 在 很 远 的 
地 方 仍 能 用 线性 来 近似 。 这 就 是 EKF 的 非 线 性 误差 ， 也 是 它 的 主要 问题 
所 在 。 


在 优化 问题 中 ， 尽 管 我 们 也 做 一 阶 (最 速 下 降 ) ROM (高 斯 牛顿 
法 或 列 文 伯 格 一 马 夸 尔 特 方法 ) 的 近似 ， 但 每 迭代 一 次 ， 状 态 估计 发 生 
改变 之 后 ， 我 们 会 重新 对 新 的 估计 点 做 泰勒 展开 ， 而 不 像 EKF 那 样 只 在 
固定 点 上 做 一 次 泰勒 展开 。 这 就 使 得 优化 方法 适用 范围 更 广 ， 在 状态 变 
化 较 大 时 亦 能 适用 。 

3. 从 程序 实现 上 来 说 ，EKF 需 要 存储 状态 量 的 均值 和 方差 ， 并 对 它们 
进行 维护 和 更 新 。 如 果 把 路 标 也 放 进 状态 ， 由 于 视觉 SLAM 中 路 标 数量 很 
大 ， 这 个 存储 量 是 相当 可 观 的 ， 且 与 状态 量 呈 平方 增长 (因为 要 存储 协 
方差 矩阵 ) 。 因 此 ，EKF SLAM 被 普遍 认为 不 适用 于 大 型 场景 。 

由 于 EKF 存 在 这 些 明 显 的 缺点 ， 我 们 通常 认为 ， 在 同等 计算 量 的 情 
况 下 ， 非 线性 优化 能 取得 更 好 的 效果 中 。 下 面 我 们 来 讨论 以 非 线性 优化 
为 主 的 后 端 。 我 们 将 主要 介绍 图 优化 ， 并 用 g2o 和 Ceres 体 验 一 下 后 端的 例 
To 


10.2 BA 与 图 优化 


如 果 你 做 过 视觉 三 维 重 建 ， 那 么 应 该 对 这 个 概念 再 熟悉 不 过 了 。 所 
谓 的 Bundle Adjustment" ， 是 指 从 视觉 重建 中 提炼 出 最 优 的 3D 模 型 和 相 
机 参数 (内 参数 和 外 参数 ) 。 从 每 一 个 特征 点 反射 出 来 的 几 束 光线 
(bundles of light rays) ， 在 我 们 把 相机 姿态 和 特征 点 空间 位 置 做 出 最 优 


的 调整 (adjustment) 之 后 ， 最 后 收 束 到 相机 光 心 的 这 个 过 程 29 ， 简 称 为 
BA。 


在 图 优化 框架 的 视觉 SLAM 算 法 里 ，BA 起 到 了 核心 作用 。 它 类 似 于 
求解 只 有 观测 方程 的 SLAM 问 题 。 在 最 近 几 年 视觉 SLAM 理 论 的 研究 中 ， 
BA 算法 不 仅 具 有 很 高 的 精度 ， 也 开始 具备 良好 的 实时 性 ， 能 够 应 用 于 在 
线 计 算 的 SLAM 场 景 中 。 而 在 21 世 纪 早 期 ， 虽 然 计 算 机 视觉 领域 的 研究 者 
们 已 经 开始 利用 BA 进行 重 构 ， 但 SLAM 的 研究 者 们 通常 认为 包含 大 量 特 
征 点 和 相机 位 姿 的 BA 计算 量 过 大 ， 不 适合 实时 计算 。 直 到 近 十 年 ， 人 们 
逐渐 认识 到 SLAM 问 题 中 BA 的 稀疏 特性 ， 才 使 它 能 够 在 实时 的 场景 中 应 
用 820 。 因 此 ， 掌 握 好 Bundle Adjustment, RAMRAPHNBICM REA 
节 ， 是 做 好 视觉 SLAM 的 关键 。 


本 节 结 合 第 5 讲 和 第 6 讲 的 内 容 ， 介 绍 Bundle Adjustment 的 模型 ， 并 且 
和 读者 和 逐步 探讨 求解 过 程 ， 特 别 是 探讨 它 的 稀 足 性 ， 然 后 介绍 一 种 通用 
的 快速 求解 方法 。 


10.2.1 “投影 模型 和 BA 代价 函数 


在 第 5 讲 里 ， 我 们 曾 介绍 了 投影 模型 和 畸变 ， 让 我 们 复习 一 下 整个 投 
影 的 过 程 。 从 一 个 世界 坐标 系 中 的 点 p 出 发 ， 把 相机 的 内 外 参数 和 畸变 都 
考虑 进来 ， 最 后 投影 成 像素 坐标 ， 需 要 如 下 步骤 。 


1. 首 先 ， 把 世界 坐标 转换 到 相机 坐标 ， 这 里 将 用 到 相机 外 参数 (R,t ): 


P' = Rp+t = |X',Y', Z". (10.36) 
2. 然 后 ， 将 P 投 至 归 一 化 平面 ， 得 到 归 一 化 坐标 : 
P, = [uc, ve, 1]! = [X'/Z',Y'/Z',1]". (10.37) 


3. 考 虑 归 一 化 坐标 的 畸变 情况 ， 得 到 去 畸变 前 的 原始 像素 坐标 。 这 里 
暂时 只 考虑 径 向 畸变 : 


ul, = ue (1 + kir? + korf 
| (tki are) (10.38) 


UL = Ue (1+ kir? + ker?) l 


4. 最 后 ， 根 据 内 参 模型 ， 计 算 像素 坐标 : 


Us = fru cr 
i . (10.39) 


Us = fyvs + Cy 
这 一 系列 计算 流程 看 似 有 些 复杂 。 我 们 用 流程 图 10-2 形 象 化 地 表示 整 
个 过 程 ， 以 帮助 读者 理解 。 读 者 应 该 能 领会 到 ， 这 个 过 程 也 就 是 前 面 讲 
的 观测 方程 ， 之 前 我 们 把 它 抽 象 地 记 成 : 


z = h(z,y). (10.40) 


a 1 ' ' 5 ' 
P'=Rp+t u =u (l+kr+kr* ) u.=fute. av. 
c c c 1° c pad clc s "E i: x s?"s 


P. =[P; IPP 1B 1| v, =v, (1 +khr +k’) v, = fv, +e, 
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图 10-2 ”计算 流程 示意 图 。 左 侧 的 p 是 全 局 坐标 系 下 的 三 维 坐标 点 ， 右 侧 的 v , ,v , 是 该 点 在 
图 像 平 面 上 的 最 终 像 素 坐标 。 中 间 畸 变 模块 中 的 =vz tee, 

现在 ， 我 们 给 出 了 它 的 详细 参数 化 过 程 。 有 具体 地 说 ， 这 里 的 x 指 代 此 
时 相机 的 位 姿 ， 即 外 参 R,t ， 它 对 应 的 李 代 数 为 E。 路 标 y 即 这 里 的 三 维 点 
p ， 而 观测 数据 则 是 像素 坐标 z^ =[u. ,v ] "。 以 最 小 二 乘 的 角度 来 考虑 ， 
那么 可 以 列 写 关 于 此 次 观测 的 误差 : 

e =z-h (é,p ). (10.41) 

然后 ， 把 其 他 时 刻 的 观测 量 也 考虑 进来 ， 我 们 可 以 给 误差 添加 一 个 
Fino iz, 为 在 位 姿 & 处 观察 路 标 p 产生 的 数据 ， 那 么 整体 的 代价 函数 

(Cost Function) 为 


m n 


SD D leyl? = 5 2 lzy aE. pI (10.42) 
对 这 个 最 小 二 乘 进行 求解 ， 相 当 于 对 位 次 和 路 标 同时 做 了 调整 ， 也 
就 是 所 谓 的 BA。 接 下 来 ， 我 们 会 根据 该 目标 函数 和 第 6 讲 介绍 的 非 线性 
优化 内 容 ， 逐 步 深入 探讨 该 模型 的 求解 。 


10.2.2 ”BA 的 求解 


观察 在 上 一 节 中 的 观测 模型 h (&,p )， 很 容易 判断 该 族 数 不 是 线性 阔 
数 。 所 以 我 们 希望 使 用 6.2 节 介绍 的 一 些 非 线 性 优化 手段 来 优化 它 。 根 据 
非 线 性 优化 的 思想 ， 我 们 应 该 从 某 个 初始 值 开 始 ， 不 断 地 寻找 下 降 方向 
Ax 来 找到 目标 水 数 的 最 优 解 ， 即 不 断 地 求解 增 量 方程 (6.22) 中 的 增 量 Ax 
。 尽 管 误差 项 都 是 针对 单个 位 次 和 路 标点 的 ， 但 在 整体 BA 目标 函数 上 ， 
我 们 必须 把 自 变量 定义 成 所 有 待 优化 的 变量 : 

g = [61,..., Em, P1,- --,Pn]”. (10.43) 

相应 地 ， 增 量 方程 中 的 Ax 则 是 对 整体 自 变 量 的 增 量 。 在 这 个 意义 

下 ， 当 我 们 给 自 变量 一 个 增 量 时 ， 目 标 钞 数 变 为 


m n 


1 il 
7 |f(z + Axs)? ~ 5 方 六 ley + Fig AG + Ei;Apjl|”. (10.44) 


i=1 j=1 
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表示 该 函数 对 路 标点 位 置 的 偏 导 。 我 们 曾 在 7.7.3 节 中 介绍 了 它们 的 具体 
形式 ， 所 以 这 里 就 不 再 展开 推导 了 。 现 在 ， 把 相机 位 次 变量 放 在 一 起 : 


£e = (€1,£0,-.-,Em]' € R™, (10.45) 
并 把 空间 点 的 变量 也 放 在 一 起 : 

Lp = [P1,P2,---,Pn]' ER”, (10.46) 
那么 ， 式 (10.44) 可 以 简化 表达 如 下 : 


1 1 
51f(z+Az) = 5 le + FAx. + EAg,|”. (10.47) 


需要 注意 的 是 ， 该 式 从 一 个 由 很 多 个 小 型 二 次 项 之 和 ， 变 成 了 一 个 
更 整体 的 样子 。 这 里 的 雅 可 比 矩 阵 E 和 F 必须 是 整体 目标 函数 对 整体 变量 
的 导数 ， 它 将 是 一 个 很 大 块 的 矩阵 ， 而 里 头 每 个 小 分 块 ， 需 要 由 每 个 误 
差 项 的 导数 F, 和 已 “拼凑 * 起 来 。 然 后 ， 无 论 我 们 使 用 高 斯 牛顿 法 还 是 列 
文 伯 格 一 马 夸 尔 特 方法 ， 最 后 都 将 面 对 增 量 线性 方程 : 


HAzx = 9. (10.48) 


根据 第 6 讲 的 知识 ， 我 们 知道 高 斯 牛顿 法 和 列 文 伯 格 一 马 夸 尔 特 方法 
的 主要 差别 在 于 ， 这 里 的 互 是 取 Jz7J 还 是 JzJ+MT 的 形式 。 由 于 我 们 把 变 
量 归 类 成 了 位 姿 和 空间 点 两 种 ， 所 以 雅 可 比 和 矩 阵 可 以 分 块 为 


J =(|F E]. (10.49) 


那么 ， 以 高 斯 牛顿 法 为 例 ， 则 坊 矩阵 为 


(10.50) 


Pa oe ae 


E'F E'E 


当然 在 列 文 但 格 一 马 夸 尔 特 方 法 中 我 们 也 需要 计算 这 个 窃 阵 。 不 难 
发 现 ， 因 为 考虑 了 所 有 的 优化 变量 ， 这 个 线性 方程 的 维度 将 非常 大 ， 包 
含 了 所 有 的 相机 位 次 和 路 标点 。 尤 其 是 在 视觉 SLAM 中 ， 一 幅 图 像 就 会 提 
出 数 百 个 特征 点 ， 大 大 增加 了 这 个 线性 方程 的 规模 。 如 果 直 接 对 五 求 逆 
来 计算 增 量 方程 ， 由 于 和 矩阵 求 逆 是 复杂 度 为 O(n ERE, ， 这 是 非常 
消耗 计算 资源 的 。 季 运 的 是 ， 这 里 的 五 矩阵 是 有 一 定 的 特殊 结构 的 。 利 
用 这 个 特殊 结构 ， 我 们 可 以 加 速 求解 过 程 。 


10.2.3 ”稀疏 性 和 边缘 化 


21 世 纪 视 觉 SLAM 的 一 个 重要 进展 是 认识 到 了 和 矩阵 互 ARA, H 
发 现 该 结构 可 以 自然 、 显 式 地 用 图 优化 来 表示 2 。 本 节 将 详细 讨论 一 
下 该 矩阵 稀 疏 结构 。 

互 和 矩阵 的 稀疏 性 是 由 雅 可 比 和 矩阵 J(x ) 引 起 的 。 考 虑 这 些 代 价 函 数 当 
中 的 其 中 一 个 e, 。 注 意 到 ， 这 个 误差 项 只 描述 了 在 5 看 到 p 这 件 事 ， 只 涉 
及 第 i 个 相机 位 姿 和 第 j 个 路 标点 ， 对 其 余部 分 的 变量 的 导数 都 为 0。 所 以 
该 误差 项 对 应 的 雅 可 比 和 矩阵 有 下 面 的 形式 : 


Oeij Oei; 
Jij(x) = (02x. .….02x6， BE, 0axe， .…02x3,.….02x3, Bp, ,02xs， 02x: . (10:51) 
i J 


其 中 0,，. 。 表示 维度 为 2x 6 的 0 矩阵 ， 同 理 0,,. ; 也 是 一 样 。 该 误差 项 对 
相机 姿态 的 偏 导 0e, /65 维度 为 2x 6， 对 路 标点 的 偏 导 0e; /Op, 维度 是 2x 


3。 这 个 误差 项 的 雅 可 比 窍 阵 ， 除 了 这 两 处 为 非 零 块 之 外 ， 其 余地 万 都 为 
零 。 这 体现 了 该 误差 项 与 其 他 路 标 和 轨迹 无 关 的 特性 。 那 么 ， 它 对 增 量 
方程 有 何 影 响 ? HRA AS EREE? 


以 图 10-3 为 例 ， 我 们 设 . 只 在 ij 处 有 非 零 块 ， 那 么 它 对 玉 的 贡献 为 


Jj, ABB LCP BAMA. KPI Jy 矩阵 也 仅 有 4 个 非 零 块 ， 位 于 
(ii ); (ij ); (i J Gi Jo 
对 于 整体 的 所 , AF: 


H= D I dij, (10.52) 


请 注意 ;在 所 有 相机 位 资中 取 值 , j 在 所 有 路 标点 中 取 值 。 我 们 把 万 
进行 分 块 : 


H, H 
H= | ics | (10.53) 


t t 
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图 10-3” 当 某 个 误差 项 具有 稀疏 性 时 ， 它 对 互 的 贡献 也 具有 稀疏 形式 。 


MBH, 只 和 相机 位 次 有 关 ， 而 理 ,, 只 和 路 标点 有 关 。 当 我 们 遍历 i;j 
时 ， 以 下 事实 总 是 成 立 的 : 


| 
| 
J=- 口 口 国 口 口 口 国 口 ,H+=' 口 品 
| 
| 
| 


1. 不 管 i SAR, H, BENAR, REH, WIE, 
LAE, H, BENA, REH AIER. 


3. FH p MH no CHIARA, EREA, MAAN 
观测 数据 而 定 。 

这 显示 了 互 的 稀 玻 结构 。 之 后 对 线性 方程 的 求解 中 ， 也 正 需要 利用 
它 的 稀 踊 结构 。 也 许 读者 还 没有 很 好 地 领会 这 里 的 意思 ， 我 们 举 一 个 实 
例 来 直观 说 明 它 的 情况 。 假 设 一 个 场景 内 有 2 个 相机 位 资 〈《C ,,C，) 和 6 


个 路 标 (P,P, P3 P,P; P) 。 这 些 相机 和 点 云 所 对 应 的 变量 为 € i 
=1, 2 及 p; j =1,…, 6。 相 机 C, 观测 到 路 标 P | ,P ,,P ,,P,， 相 机 C ,观测 到 
路 标 P P,P; ,P 。。 我 们 把 这 个 过 程 画 成 示意 图 10-4。 相 机 和 路 标 以 圆 


形 节点 表示 。 如 果 i 相机 能 够 观测 到 j 点 云 ， 我 们 就 在 它们 对 应 的 节点 连 
上 一 条 边 。 


1 2 2 2 2 2 2 2 2 
5 (len? + llerall? + llersll? + leial? + lleasll” + lezali? + [leas I? + lleze|l”) . 
(10.54) 


图 10-4 ”点 和 边 组 成 的 示意 图 。 该 图 显示 相机 C , 观测 到 了 路 标点 P , ,P , PP,’ HC, 
看 到 了 P ,到 P,。 
这 里 的 e; 使 用 之 前 定义 过 的 代价 函数 ， 即 了 式 (10.42)。 以 e 1 AGI, € 
描述 了 在 C , 看 到 了 P , 这 件 事 ， 与 其 他 的 相机 位 资 和 路 标 无 关 。 令 J He 


11 所 对 应 的 雅 可 比 和 矩阵 ， 不 难看 出 e , 对 相机 变量 上 5, 和 路 标点 p , …,p 6 的 
偏 导 都 为 0。 我 们 把 所 有 变量 以 x = , ,& , ,p , ,…,p ，) 的 顺序 摆 放 ， 则 有 : 


Ji = 


Oell (2 Oe 


11 
Dx 025,025.35 02x3, 02x: 02x3 ) 2 (10.55) 


BE, A Gp, 
为 了 方便 表示 稀疏 性 ， 我 们 用 带 有 颜色 的 方块 表示 和 矩阵 在 该 方块 内 
有 数值 ， 其 余 没有 颜色 的 区 域 表 示 矩 阵 在 该 处 数值 都 为 0。 那 么 上 面 的 J ， 


则 可 以 表示 成 图 10-5 所 示 图 案 。 同 理 ， 其 他 的 雅 可 比 矩 阵 也 会 有 类 似 的 入 
Fito 
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图 10-5 J,, 矩阵 的 非 零 块 分 布 图 。 上 方 的 标记 表示 矩阵 该 列 所 对 应 的 变量 。 由 于 相机 参数 
维 数 比 点 云 参数 维 数 要 大 ， 所 以 C , 对 应 的 矩阵 块 要 比 P, 对 应 的 矩阵 块 宽 一 些 。 
为 了 得 到 该 目标 阔 数 对 应 的 雅 可 比 和 矩阵 ， 我 们 可 以 将 这 些 方 按照 一 


定 顺 序列 为 向 量 ， 那 么 整体 雅 可 比 和 矩阵 及 相应 的 五 矩阵 的 稀 跻 情况 就 如 
图 10-6 所 示 。 
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图 10-6 ” 雅 可 比 矩 阵 的 稀疏 性 (Æ) MIH ABRAM (6) ， 填 色 的 方块 表示 矩阵 在 对 应 
的 矩阵 块 处 有 数值 ， 其 余 没有 颜色 的 部 分 表示 矩阵 在 该 处 的 数值 始终 为 0。 


也 许 你 已 经 注意 到 了 ， 图 10-4 对 应 的 邻接 矩阵 (Adjacency Matrix) 
5 和 上 图 中 的 互 矩 阵 ， 除 了 对 角 元 素 以 外 的 其 余部 分 有 着 完全 一 致 的 结 
构 。 事 实 上 的 确 如 此 。 上 面 的 互 矩 阵 一 共有 8x 8 个 和 矩阵 块 ， 对 于 五 矩阵 当 
中 处 于 非 对 角 线 的 矩阵 块 来 说 ， 如 果 该 矩阵 块 非 零 ， 则 其 位 置 所 对 应 的 
变量 之 间 在 图 中 会 存在 一 条 边 ， 我 们 可 以 从 图 10-7 中 清晰 地 看 到 这 一 点 。 
所 以 ， 互 矩阵 当中 的 非 对 角 部 分 的 非 零 矩阵 块 可 以 理解 为 其 对 应 的 两 个 
变量 之 间 存 在 联系 ， 或 者 可 以 称 之 为 约束 。 于 是 ， 我 们 发 现 图 优化 结构 
与 增 量 方 程 的 稀疏 性 存在 着 明显 的 联系 。 
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图 10-7 五 矩 阵 中 非 零 矩阵 块 和 图 中 边 的 对 应 关系 。 如 左 图 矩阵 中 右 侧 的 红色 矩阵 块 ， 表 
示 在 右 图 中 其 对 应 的 变量 C , 和 P ,之 间 存在 一 条 边 e ,。。 


现在 考虑 更 一 般 的 情况 ， 假 如 我 们 有 m 个 相机 位 姿 ，n 个 路 标点 。 由 
于 通常 路 标的 数量 远 远 多 于 相机 ， 于 是 有 7 之 m 。 由 上 面 的 推理 可 知 ， 实 
际 的 互 矩 阵 会 如 图 10-8 所 示 。 它 的 左上 角 块 显得 非常 小 ， 而 右 下 角 的 对 角 
块 占据 了 大 量 地 方 。 除 此 之 外 ， 非 对 角 部 分 则 分 布 着 散乱 的 观测 数据 。 
由 于 它 的 形状 很 像 箭头 ， 又 称 为 箭头 形 (Arrow-like) FER, AB, € 
也 很 像 一 把 久子 ， 所 以 笔者 也 称 其 为 饮 形 和 矩阵。 


mc+ns 


图 10-8 “一般 情况 下 的 五 矩阵 。 


对 于 具有 这 种 稀疏 结构 的 五 ， 线 性 方程 HAx =g 的 求解 会 有 什么 不 同 
呢 ? 现实 当中 存在 着 若干 种 利用 互 的 稀 足 性 加 速 计 算 的 方法 ， 丁 介绍 
视觉 SLAM 里 一 种 最 常用 的 手段 : Schur 消 元 。 在 SLAM 人 研究 中 亦 称 为 
Marginalization (边缘 化 ) o 


仔细 观察 一 下 图 10-8， 我 们 不 难 发 现 这 个 矩阵 可 以 分 成 4 个 块 ， 和 式 
(10.53) 一 致 。 左 上 角 为 对 角 块 矩阵 ， 每 个 对 角 块 元 素 的 维度 与 相机 位 姿 
的 维度 相同 ， 且 是 一 个 对 角 块 矩阵 。 右 下 角 也 是 对 角 块 矩阵 ， 每 个 对 角 
块 的 维度 是 路 标的 维度 。 非 对 角 块 的 结构 与 具体 观测 数据 相关 。 我 们 首 
先 将 这 个 矩阵 按照 图 10-9 所 示 的 方式 做 区 域 划 分 ， 读 者 不 难 发 现 ， 这 4 个 
区 域 正好 对 应 了 公式 (10.50) 中 的 4 个 和 矩阵 块 。 为 了 后 续 分 析 方 便 ， 我 们 记 
这 4 个 块 为 B,E,E7,C。 
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图 10-9 互 矩 阵 的 区 域 划 分 。 
于 是 ， 对 应 的 线性 方程 组 也 可 以 由 五 Ax =g 变 为 如 下 形式 : 


ATe 
AG, 


B E v 
= : (10.56) 


其 中 B 是 对 角 块 矩阵 ， 每 个 对 角 块 的 维度 和 相机 参数 的 维度 相同 ， 对 
角 块 的 个 数 是 相机 变量 的 个 数 。 由 于 路 标 数量 会 远 远 大 于 相机 变量 个 
数 ， 所 以 C 往往 也 远大 于 B 。 三 维 空间 中 每 个 路 标点 为 三 维 ， 于 是 C 矩阵 
为 对 角 块 矩阵 ， 每 个 块 为 3x 3 和 矩阵。 对 角 块 矩阵 求 逆 的 难度 远 小 于 对 一 
般 和 矩阵 的 求 逆 难度 ， 因 为 我 们 只 需要 对 那些 对 角 线 矩阵 块 分 别 求 逆 即 
可 。 考 虑 到 这 个 特性 ， 我 们 对 线性 方程 组 进行 高 斯 消 元 ， 目 标 是 消去 右 
上 角 的 非 对 角 部 分 已 ， 得 : 


B E Ax. 
ET C Azp 0 I 


I -EC™ 
0 I 


VU 
| (10.57) 
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整理 ， 得 : 


Aze 
ATp 


(10.58) 


| v — EC !w 


B-—EC'E' 0 
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经 过 消 元 之 后 ， 方 程 组 第 一 行 变 成 和 Ax, 无 关 的 项 。 单 独 把 它 拿 
来 ， 得 到 关于 位 姿 部 分 的 增 量 方程 : 

[B — ECT! E"] Ax, = v — EC ww. (10.59) 

这 个 线性 方程 的 维度 和 B Elt ROEE RENDE, 
然后 把 解 得 的 Ax. 代入 到 原 方程 ， 然 后 求解 Ax, 。 这 个 过 程 称 为 
Marginalization'™! ， 或 者 Schur 消 元 (Schur Elimination) 。 相 比 于 直接 解 
线性 方程 的 做 法 ， 它 的 优势 在 于 : 

1. 在 消 元 过 程 中 ， 由 于 C 为 对 角 块 ， 所 以 C! 容易 解 出 。 

2. 求 解 了 Ax, 之 后 ， 路 标 部 分 的 增 量 方程 由 Ax, =C | (w-E T Ax, ) 给 出 。 
这 依然 用 到 了 C! 易于 求解 的 特性 。 


于 是 ， 边 绿化 的 主要 计算 量 在 于 求解 式 (10.59)。 天 于 这 个 方程 ， 我 
们 能 说 的 就 不 多 了 。 它 仅 是 一 个 普通 的 线性 方程 ， 没 有 特殊 的 结构 可 以 
利用 。 我 们 将 此 方程 的 系数 记 为 S ， 它 的 稀疏 性 如 何 呢 ? 图 10-10 显 示 了 
进行 Schur 消 元 之 后 的 一 个 S LG, Wa BCH MET SAA A. 


mc 
110-10 “对 五 矩阵 进行 Schur 消 元 后 S 矩阵 的 稀疏 状态 。 


前 面 说 到 ， 玉 矩阵 的 非 对 角 块 处 的 非 零 元 素 对 应 着 相机 和 路 标的 关 
联 。 那 么 ， 进 行 了 Schur 消 元 后 5 的 稀 下 性 是 否 具有 物理 意义 呢 ? 答 案 是 
肯定 的 。 此 处 我 们 不 加 证 明 地 说 ，S 矩阵 的 非 对 角 线 上 的 非 零 和 矩阵 块 ， 表 
示 了 该 处 对 应 的 两 个 相机 变量 之 间 存 在 着 共同 观测 的 路 标点 ， 有 时 候 称 
为 共 视 (Co-visibility) 。 反 之 ， 如 果 该 块 为 零 ， 则 表示 这 两 个 相机 没有 
共同 观测 。 例 如 图 10-11 所 示 的 稀 玻 和 矩阵， 左上 角 前 4x 4 个 矩阵 块 可 以 表 
示 对 应 的 相机 变量 C , ,C , ,C , ,C , 之 间 有 共同 观测 。 


图 10-11 LAS 矩阵 中 前 4x 4 个 矩阵 块 为 例 ， 这 个 区 域 当中 的 矩阵 块 9 ,,S ,, 不 为 零 ， 表 示 相 
机 C , 和 相机 C , 、C , 之 间 有 共同 观测 点 ; 而 $ , 为 零 则 表示 C , 和 C , 之 间 没 有 共同 观测 的 路 
标 。 


于 是 ，S 和 矩阵 的 稀疏 性 结构 当 取 决 于 实际 观测 的 结果 ， 我 们 无 法 提前 
预知 。 在 实践 当中 ,例如 ORB-SLAMr3 中 的 Local Mapping 环 节 ， 在 做 
BA 的 时 候 刻 意 选 择 那些 具有 共同 观测 的 帧 作为 关键 帧 ， 在 这 种 情况 下 
Schur 消 元 后 得 到 的 S 就 是 稠密 矩阵。 不 过 ， 由 于 这 个 模块 并 不 是 实时 执 
行 ， 所 以 这 种 做 法 也 是 可 以 接受 的 。 但 是 在 另 一 些 方法 里 面 ， 例 如 
DSOEa 、OKVIS 等 ， 它 们 采用 了 滑动 窗口 方法 (Sliding Window) o 
这 类 方法 对 每 一 帧 都 要 求 做 一 次 BA 来 防止 误差 的 累积 ， 因 此 它们 也 必须 
采用 一 些 技巧 来 保持 S 和 矩阵 的 稀疏 性 。 读 者 如 果 希 望 能 够 更 加 深入 这 一 
块 ， 可 以 参考 它们 的 论文 。 这 里 就 不 谈 这 些 过 于 细节 的 事情 了 。 


从 概率 角度 来 看 ， 我 们 称 这 一 步 为 边缘 化 ， 是 因为 我 们 实际 上 把 求 
(Ax,, Ax, ) 的 问题， 转化 成 了 先 求 Ax. ， 再 求 Ax 的 过 程 。 这 一 步 相 当 于 做 
了 条 件 概率 展开 : 


P( Ge; 8p) =P (£e) + Pepe): (10.60) 


结果 是 求 出 了 关于 x, 的 边缘 分 布 ， 故 称 边 缘 化 。 在 前 面 讲 的 边缘 化 
过 程 中 ， 我 们 实际 把 所 有 的 路 标点 都 给 边缘 化 了 。 根 据 实际 情况 ， 我 们 
也 能 选择 一 部 分 进行 边缘 化 。 同 时 ，Schur 消 元 只 是 实现 边缘 化 的 其 中 一 
种 方式 ， 同 样 可 以 使 用 Cholesky 分 解 进行 边缘 化 。 

读者 可 能 会 继续 问 ， 在 进行 了 Schur 消 元 后 ， 我 们 还 需要 求解 线性 方 
旦 组 (10.59)。 对 它 的 求解 是 否 还 有 什么 技巧 呢 ? 很 遗憾 ， 这 部 分 束 属 于 
传统 的 矩阵 数值 求解 了 ， 通 常 是 用 分 解 来 计算 的 。 不 管 采用 哪 种 求解 办 
法 ， 我 们 都 建议 利用 互 的 稀 芷 性 进行 Schur 消 元 。 不 光 是 因为 这 样 可 以 提 
高 速度 ， 也 因为 消 元 后 的 S 矩阵 的 条 件数 往往 比 之 前 的 五 德 阵 要 小 。 
Schur 消 元 也 并 不 意味 着 将 所 有 路 标 消 元 ， 将 相机 变量 消 元 也 是 SLAM 当 
中 采用 的 手段 。 


10.2.4” 鲁 棒 核 函数 


在 前 面 的 BA 问题 中 ， 我 们 最 小 化 误差 项 的 二 荡 数 平方 和 作为 目标 消 
数 。 这 种 做 法 虽然 很 直观 ， 但 存在 一 个 严重 的 问题 : 如 果 出 于 误 匹 配 等 
原因 ， 某 个 误差 项 给 的 数据 是 错误 的 ， 会 发 生 什 么 呢 ? 我 们 把 一 条 原本 
不 应 该 加 到 图 中 的 边 给 加 进去 了 ， 然 而 优化 算法 并 不 能 辨别 出 这 是 个 错 
误 数 据 ， 它 会 把 所 有 的 数据 都 当 作 误 差 来 处 理 。 这 时 ， 算 法 会 看 到 一 条 
误差 很 大 的 边 ， 它 的 梯度 也 很 大 ， 意 味 着 调整 与 它 相 关 的 变量 会 使 目标 
疯 数 下 降 更 多 。 所 以 ， 算 法 将 试图 调整 这 条 边 所 连接 的 节 和 点 的 估计 值 ， 
使 它们 顺应 这 条 边 的 无 理 要 求 。 由 于 这 条 边 的 误差 真 的 很 大 ， 往 往 会 抹 
平 了 其 他 正确 边 的 影响 ， 使 优化 算法 专注 于 调整 一 个 错误 的 值 。 这 显然 
不 是 我 们 希望 看 到 的 。 

出 现 这 种 问题 的 原因 是 ， 当 误差 很 大 时 ， 二 范 数 增长 得 太 快 。 于 是 
就 有 了 核 遂 数 的 存在 。 核 水 数 保证 每 条 边 的 误差 不 会 大 得 没 边 而 掩盖 掉 
其 他 的 边 。 具 体 的 方式 是 ， 把 原先 误差 的 二 范 数 度量 替换 成 一 个 增长 没 
有 那么 快 的 水 数 ， 同 时 保证 自己 的 光滑 性 质 (不 然 无 法 求 导 ! ) 。 因 为 
它们 使 得 整个 优化 结果 更 为 稳健 ， 所 以 又 叫 它 们 和 鲁 棒 核 国 数 (Robust 
Kernel) 。 


鲁 棱 核 水 数 有 许多 种 ， 例 如 最 常用 的 Huber 核 : 


ze 当 |e| < ô, 
(10.61) 
6 (le| — 48) 其 他 

我 们 看 到 ， 当 误差 e 大 于 某 个 阅 值 5 后， 函数 增长 由 二 次 形式 变 成 了 
一 次 形式 ， 相 当 于 限制 了 梯度 的 最 大 值 。 同 时 ，Huber 核 阔 数 又 是 光滑 
的 ， 可 以 很 方便 地 求 导 。 图 10-12 显 示 了 Huber 核 水 数 与 二 次 遂 数 的 对 比 ， 
可 见 在 误差 较 大 时 Huber 核 阔 数 增长 明显 低 于 二 次 国 数 。 


图 10-12 ”Huber 核 水 数 。 


除了 Huber 核 之 外 ， 还 有 Cauchy 核 、Tukey 核 ， 等 等 ， 读 者 可 以 看 看 
g20 和 Ceres 都 提供 了 哪些 核 疯 数 。 


10.2.5 “小 结 


丁 我 们 重点 介绍 了 BA 中 的 稀 足 性 问题 。 不 过 ， 实 践 当 中 ， 有 许多 
软件 库 为 我 们 提供 了 细节 操作 ， 而 我 们 需要 做 的 主要 是 构造 Bundle 
Adjustment 问 题 ， 设 置 Schur 消 元 ， 然 后 调用 稠密 或 者 稀 跑 和 矩阵 求解 器 对 
变量 进行 优化 即 可 。 如 果 读 者 希望 更 深入 地 了 解 BA， 可 以 在 阅读 完 本 节 
的 基础 上 ， 进一步 参考 文献 [26] 学 习 。 

下 面 的 两 节 ， 我 们 将 使 用 g2o 和 Ceres 两 个 库 来 做 Bundle Adjustment. 
为 了 体现 出 它们 的 区 别 ， 我 们 会 让 这 个 程序 共用 很 多 代码 。 共 用 的 代码 
主要 在 common 文 件 夹 下 ， 其 他 的 文件 则 不 同 。 我 们 将 使 用 公开 的 数据 库 
[75] 当 中 的 文件 来 进行 编程 练习 。 


10.3 ”实践 : g20 
10.3.1 BA 数据 集 


在 本 讲 代码 目录 下 的 common 文 件 夹 是 两 个 实验 共有 的 数据 集 部 分 。 
它 的 布局 如 图 10-13 所 示 。 其 中 ，fl ags 文 件 夹 下 的 两 个 文件 定义 了 


CommandArgs 这 个 类 ， 该 类 用 来 解析 用 户 输 入 的 参数 ， 同 时 也 对 程序 需 
要 的 参数 提供 默认 值 及 文档 说 明 。BundleParams 这 个 类 定义 了 Bundle 
Adjustment 使 用 的 所 有 参数 ， 也 调用 了 CommandArgs 类 型 的 变量 。 由 于 
CommandArgs 这 个 类 的 存在 ， 我 们 可 以 直接 在 程序 后 面 使 用 -help 来 查看 
程序 所 有 参数 的 含义 ， 使 用 方式 可 以 参考 该 程序 中 BundleParams 类 型 的 写 


法 。 


4 COMMON 
4 flags 
command_args.cpp 
command_args.h 
4 tools 
random.h 
rotation.h 
BALProblem.cpp 
BALProblem.h 


BundleParams.h 


projection.h 


10-13. common XF XE #2444. 


tools 是 一 些 数 学 工具 函数 ， 读 者 可 自行 查看 。 我 们 的 相机 和 路 标 参 
数 与 前 面 提 到 的 代价 函数 ， 即 式 (10.42) 保 持 一 致 ， 不 过 在 实验 中 我 们 要 
把 相机 内 参 也 当 作 优化 变量 考虑 。 我 们 用 fk , ,来 表示 焦距 和 畸变 ，3 个 
参数 表示 平移 ，3 个 参数 表示 旋转 (于 是 相机 一 共有 9 个 未 知 量 ) ， 路 标 
也 使 用 三 维 坐 标 来 表示 。projection.h 是 前 面 提 到 的 投影 计算 流程 图 10-2 的 
代码 实现 。 由 于 g2o 和 ceres 都 会 用 到 这 个 投影 模型 ， 因 此 这 个 文件 也 将 被 
两 个 工程 所 共享 。 

除 此 之 外 ，BALProblem 这 个 类 将 保存 我 们 的 BundleAdjustment 所 需 
要 的 所 有 数据 ， 其 中 就 包括 了 相机 和 路 标 之 间 的 关联 ， 以 及 相机 变量 和 
路 标 变 量 的 初始 值 ， 这 些 数据 的 原始 信息 都 保存 在 data 文 件 夹 下 的 TXT 文 
件 当 中 。 我 们 可 以 打开 data/problem-16-22106-pre.txt 查 看 文件 信息 : 


1 |16 22106 83718 

2 |0 0 -3.859900e+02 3.871200e+02 
3 {10 -3.844000e+01 4.921200e+02 
4 |2 0 -6.679200e+02 1.231100e+02 
5 |7 0 -5.991800e+02 4.079300e+02 


它 的 第 一 行 记 录 了 相机 数量 、 路 标 效 量 和 总 的 观测 数量 。 从 第 二 行 
起 ， 分 别 记录 第 i 个 相机 观测 第 j 个 路 标 所 看 到 的 像素 坐标 。 这 就 构成 了 
一 个 BA 问题 ， 亦 可 看 成 由 纯 观测 方程 组 成 的 SCAM 问 题 。 除 此 之 外 ， 
BALProblem 也 提供 了 将 数据 导出 到 PLY 文 件 这 样 的 功能 ， 方 便 用 户 通 过 
Meshlab 软 件 查 看 点 云 的 三 维 信息 。 


10.3.2”g20 求 解 BA 


下 面 来 考虑 如 何 使 用 g2o 求 解 这 个 BA 问题 。 和 以 前 一 样 ，g2o 使 用 图 
模型 来 描述 问题 的 结构 ， 所 以 我 们 要 用 节点 来 表示 相机 和 路 标 ， 然 后 用 
边 来 表示 它们 之 间 的 观测 。 我 们 仍然 使 用 自 定义 的 点 和 边 ， 只 需 覆 盖 一 
些 关 键 疯 数 即 可 。 针 对 相机 和 路 标 ， 我 们 可 以 定义 如 下 继承 ， 使 用 
override 关 键 字 来 表示 对 基 类 虚 图 数 的 覆盖 : 


kÀ slambook/ch10/ £20_custombundle/g20_bal_class.h 
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34 


35 


36 


37 


38 


#include "g2o0/core/base_vertex.h" 


#include "g2o/core/base_binary_edge.h" 


#include <Eigen/Core> 


#include "ceres/autodiff.h" 


#include "tools/rotation.h" 


#include "common/projection.h" 


// 节点 没什么 好 说 的 ， 直 接 把 更 新 量 看 作 向 量 求 加 法 即 可 
class VertexCameraBAL : public g2o::BaseVertex<9,Eigen::VectorXd> 
{ 
public: 
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 
VertexCameraBAL () {} 
virtual bool read(std::istream& is) {return false;} 
virtual bool write ( std::ostream& os ) const {return false;} 


virtual void setToOriginImp1 () {} 


virtual void oplusImpl(const double* update) override { 
Eigen: :VectorXd::ConstMapType v(update, VertexCameraBAL: : Dimension) ; 


_estimate += v; 


F; 


class VertexPointBAL : public g2o::BaseVertex<3, Eigen: :Vector3d> 
{ 
public: 
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 
VertexPointBAL () {} 
virtual bool read( std::istream& is) {return false;} 
virtual bool write(std::ostream& os) const {return false;} 
virtual void setToOriginImpl() {} 
virtual void oplusImpl(const double* update) override { 
Eigen: :Vector3d::ConstMapType v(update) ; 


_estimate += v; 


定义 了 节点 以 后 ， 还 需要 定义 边 来 表示 节点 之 间 的 联系 。 每 一 条 边 


都 对 应 了 一 个 代价 函数 (10.42)， 同 时 为 了 避免 复杂 的 求 导 运算 ， 我 们 借 
助 Ceres 库 当中 的 Autodi ff ( 即 自动 求 导 ) 功能 。 该 功能 需要 类 型 将 需要 
求 导 的 公式 实现 在 括号 运算 符 operator0 里 ， 代 码 如 下 : 


kA) slambook/ch10/ g2o0_custombundle/g20_bal_class.h 


class EdgeObservationBAL : public g2o0::BaseBinaryEdge<2, Eigen::Vector2d, 


VertexCameraBAL, VertexPointBAL>{ 

public: 
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 
EdgeObservationBAL () {} 


virtual bool read(std::istream& /*is*/){ return false;} 
virtual bool write(std::ostream& /*os*/)const { return false;} 


virtual void computeError() override // HERR BA, KM operator(it HK 
函数 


const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*>(vertex(0)) 
; 
const VertexPointBAL* point = static_cast<const VertexPointBAL*>(vertex(1)) 


》 


(*this) (cam->estimate().data(), point->estimate().data(), _error.data()); 


// 为 了 使 用 Ceres 求 导 功能 而 定义 的 函数 ， 让 本 类 成 为 拟 函 数 类 


template<typename T> 
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bool operator()( const T* camera, const T* point, T* residuals )const 


{ 


T predictions [2]; 


CamProjectionWithDistortion(camera, point, predictions) ; 


residuals[0] = predictions[0] - T(measurement() (0) ) ; 


residuals[1] = predictions[1] - T(measurement() (1) ) ; 


return true; 


virtual void linearizeOplus() override 


const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*>(vertex(0)) 
; 

const VertexPointBAL* point = static_cast<const VertexPointBAL*>(vertex(1)) 
; 

typedef ceres::internal: :AutoDiff<Edge0bservationBAL, double, 


VertexCameraBAL: :Dimension, VertexPointBAL::Dimension> BalAutoDiff; 


Eigen: :Matrix<double, Dimension, VertexCameraBAL::Dimension, Eigen:: 
RowMajor> dError_dCamera; 

Eigen: :Matrix<double, Dimension, VertexPointBAL::Dimension, Eigen: :RowMajor 
> dError_dPoint; 

double *parameters[] = { const_cast<double*>(cam->estimate() .data()), 
const_cast<double*>(point->estimate().data()) }; 

double *jacobians[] = { dError_dCamera.data(), dError_dPoint.data() }; 
double value [Dimension]; 

// Ceres 中 的 自动 求 导 函数 用 法 ， 需 要 提供 operator) 函数 成 员 

bool diffState = BalAutoDiff::Differentiate(*this, parameters, Dimension, 


value, jacobians) ; 


// copy over the Jacobians (convert row-major -> column-major) 
if (diffState) { 

_jacobianOplusXi = dError_dCamera; 
dError_dPoint; 


_jacobianOplusXxj 
} else { 
assert(0 && "Error while differentiating"); 


_jacobian0OplusXi.setZero() ; 


_jacobianOplusXi.setZero() ; 


以 上 定义 了 本 问题 中 使 用 的 节点 和 边 。 下 面 需 要 根据 BALProblem 类 
当中 的 实际 数据 来 生成 一 些 节 点 和 边 ， 交 给 g2o 进 行 优化 。 值 得 注意 的 
是 ， 为 了 充分 利用 BA 中 的 稀 足 性 ， 需 要 在 这 里 将 路 标 中 的 setMarginalized 
属性 设置 为 true。 代 码 的 主要 片段 如 下 : 


kÀ slambook/ch10/g20_custombundle/g20_bundle.cpp (片段 ) 


void BuildProblem(const BALProblem* bal_problem, g2o0::SparseOptimizer* optimizer, 


const BundleParams& params) 


{ 


const int num_points = bal_problem->num_points() ; 
const int num_cameras = bal_problem->num_cameras() ; 
const int camera_block_size = bal_problem->camera_block_size() ; 


const int point_block_size = bal_problem->point_block_size(); 


// 添加 相机 和 对 应 的 初始 值 

const double* raw_cameras = bal_problem->cameras(); 

for(int i = 0; i < num_cameras; ++i) 

{ 
ConstVectorRef temVecCamera(raw_cameras + camera_block_size * i, 
camera_block_size); 


VertexCameraBAL* pCamera = new VertexCameraBAL() ; 


pCamera->setEstimate(temVecCamera); // 初始 值 


pCamera->setId(i); 


optimizer->addVertex (pCamera) ; 


// Set point vertex with initial value in the dataset. 

const double* raw_points = bal_problem->points() ; 

// const int point_block_size = bal_problem->point_block_size(); 

for(int j = 0; j < num_points; ++j) 

{ 
ConstVectorRef temVecPoint(raw_points + point_block_size * j, 
point_block_size) ; 
VertexPointBAL* pPoint = new VertexPointBAL(); 
pPoint->setEstimate(temVecPoint); // 点 云 的 初始 值 
PPoint->setId(j + num_cameras) ; // 为 了 不 和 相机 冲突 ， 加 上 相机 的 iad 


// 设置 该 点 在 解 方程 时 进行 Schur 消 元 
pPoint->setMarginalized(true); 


optimizer->addVertex(pPoint); 


// 为 图 添加 边 
const int num_observations = bal_problem->num_observations(); 


const double* observations = bal_problem->observations(); 


for(int i = 0; i < num_observations; ++i) 
{ 
EdgeObservationBAL* bal_edge = new EdgeObservationBAL() ; 
const int camera_id = bal_problem->camera_index() [i]; // 使 用 前 面 的 id 获取 


相机 


2 const int point_id = bal_problem->point_index() [i] + num_cameras; // 使 用 前 
面 的 id 获得 点 

43 // Huber loss 函数 (默认 为 不 用 设置 ) 

44 if (params.robustify) 

45 { 

46 g20::RobustKernelHuber* rk = new g2o0::RobustKernelHuber; 

47 rk->setDelta(1.0); 

48 bal_edge->setRobustKernel (rk); 

49 } 

50 // 设置 边 所 对 应 的 两 个 节点 

51 bal_edge->setVertex(0,dynamic_cast<VertexCameraBAL*>(optimizer->vertex( 


camera_id))); 
52 bal_edge->setVertex(1,dynamic_cast<VertexPointBAL*>(optimizer->vertex ( 


point_id))); 


53 // RE Aa AE (在 此 处 为 单位 矩阵 ) 

54 bal_edge->setInformation(Eigen::Matrix2d: :Identity()); 

55 // 设置 边 所 对 应 的 观测 值 

56 bal_edge->setMeasurement (Eigen::Vector2d(observations [2*i+0] , observations 
[2*i + 1])); 

57 

58 optimizer->addEdge(bal_edge) ; 


10.3.3 R 


在 使 用 g2o 时 ， 求 解 的 设置 无 非 这 几 种 : 1. 使 用 何 种 方法 ( 列 文 伯 格 
一 马 硅 尔 特 方法 、DogLeg 等 ) 来 定义 非 线 性 优化 的 下 降 策略 ，2. 使 用 哪 
类 线性 求解 器 。 注 意 到 这 里 需要 使 用 到 稀 玻 性 ， 所 以 必须 选用 稀 踊 的 求 
解 器 。 


kÀ slambook/ch10/g20_custombundle/g20_bundle.cpp (片段 ) 


t 


ypedef g2o0::BlockSolver<g2o: :BlockSolverTraits<9,3> > BalBlockSolver; 


void SetSolverOptionsFromFlags(BALProblem* bal_problem, const BundleParams& params, 


{ 


g20::SparseOptimizer* optimizer) 


BalBlockSolver* solver_ptr; 


g2o0::LinearSolver<BalBlockSolver: :PoseMatrixType>* linearSolver = 


// 使 用 稠密 计算 方法 


if (params.linear_solver == "dense_schur" ) { 


0; 


1 linearSolver = new g20::LinearSolverDense<BalBlockSolver::PoseMatrixType>() 

12 } 

13 // Ae Rit RAK 

14 else if (params.linear_solver == "sparse_schur") { 

15 linearSolver = new g20::LinearSolverCholmod<BalBlockSolver::PoseMatrixType 
>(); 

16 // 让 solver 3) 4E it 47 HE AAR A Fh ia lE 

17 dynamic_cast<g2o: :LinearSolverCholmod<BalBlockSolver: :PoseMatrixType>* >( 
linearSolver)->setBlockOrdering (true) ; 

18 } 

19 

20 solver_ptr = new BalBlockSolver(linearSolver) ; 

21 

22 g20::OptimizationAlgorithmWithHessian* solver; 

23 if (params.trust_region_strategy == "levenberg_marquardt") { 

24 solver = new g20::0ptimizationAlgorithmLevenberg(solver_ptr); // 使 用 列 文 
伯 格 一 马 夺 尔 特 下 降 法 

25 } 

26 else if(params.trust_region_strategy == "dogleg"){ 

27 solver = new g20::OptimizationAlgorithmDogleg(solver_ptr); // 使 用 DogLeg 
FRE 

28 } 

29 else 

30 { 

31 std::cout << "Please check your trust_region_strategy parameter again.."<< 
std::endl; 

32 exit (EXIT_FAILURE) ; 

33 } 

34 

35 optimizer->setAlgorithm(solver) ; 

36 |} 


现在 问题 基本 上 搭建 好 了 。 我 们 通过 BuildProblem 函 数 完 成 了 对 目标 
遂 数 的 构造 ， 也 通过 SetSolverOptionsFromFlags 遂 数 使 用 用 户 的 输入 参数 
来 设置 优化 求解 。 剩 下 要 做 的 就 是 思考 该 如 何 求解 这 个 问题 了 。 有 了 g2o 
这 样 的 优化 库 ， 求 解 基本 上 可 以 一 步 完成 ， 这 点 跟前 面 介 绍 的 g2o 用 法 几 
平 是 一 样 的 : 


kÀ slambook/ch10/g20_custombundle/g20_bundle.cpp (片段 ) 


1 | optimizer.initializeOptimization() ; 
2 optimizer .setVerbose (true); 


3 optimizer.optimize(params.num_iterations) ; 


使 用 Meshlab 分 别 打开 g2o 执 行文 件 夹 下 的 initial.ply 和 全 nal.ply 两 个 文 
件 ， 优 化 前 后 的 效果 如 图 10-14 所 示 。 


初始 值 优化 值 


图 10-14 8g2o 优 化 结果 。 左 侧 为 优化 前 初始 值 ， 右 侧 为 优化 后 的 。 


由 于 g2o 库 本 身 公 开 API 说 明 也 少 ， 我 们 通常 只 能 通过 网 上 的 一 些 开 
源 项 目 及 其 本 身 的 源 代 码 来 了 解 它 。 这 里 需要 提醒 读者 的 是 ，g2o 在 做 
Bundle Adjustment 的 优化 时 必须 将 其 所 有 点 云 全 部 Schur 掉 ， 否 则 会 出 
fa! 其 原 A 在 于 K 们 使 用 了 
g20::LinearSolver<BalBlockSolver::PoseMatrixType> 这 个 类 型 来 指定 
linearsolver， 其 中 模板 参数 当中 的 Pose-MatrixType 在 程序 中 为 相机 姿态 参 
数 的 维度 ， 于 是 Bundle Adjustment 当 中 Schur 消 元 后 解 的 线性 方程 组 必须 


是 只 含有 相机 姿态 变量 上 9 。 

Ceres 库 则 没有 g2o 这 样 的 限制 。Ceres 给 了 开发 者 很 大 的 空间 去 操作 
自己 的 优化 策略 ， 它 在 Schur 消 元 操作 时 完全 不 需要 把 所 有 点 云 都 消 元 
掉 ， 用 户 可 以 目 己 编写 函数 选择 消 元 哪些 点 云 。 接 下 来 我 们 也 会 给 出 
Ceres 的 BA 示例 。 


10.4 XR: Ceres 


现在 我 们 再 来 看 看 换 成 Ceres 实 现 同样 的 效果 该 如 何 做 。 如 前 面 介绍 
的 那样 ，Ceres 是 针对 一 般 优 化 问题 产生 的 库 ， 因 此 它 并 没有 像 g2o 那 样 提 
供 一 些 Vertex 或 者 Edge 这 类 友好 的 接口 。 不 过 这 也 给 Ceres 带 来 了 极 大 的 
灵活 性 。 我 们 接 下 来 可 以 看 到 ，Ceres 可 以 在 原始 数组 上 进行 直接 的 优化 
操作 ， 而 g2o 则 通过 复制 的 手段 将 数据 通过 Vertex 或 者 Edge 结 构 复 制 到 
optimizer 里 面 。 相 比 之 下 ，Ceres 对 “优化 ”显得 更 贴近 一 些 。 


10.4.1 ” ”Ceres 求解 BA 


g20 用 Edge 来 保存 每 一 个 代价 函数 ， 而 Ceres 却 是 用 Problem 类 型 来 构 
建 最 终 的 目标 函数 。 与 第 6 讲 介绍 的 一 致 ， 我 们 使 用 AddResidualBlock 来 
添加 代价 函数 ， 最 后 组 成 整个 目标 国 数 。 为 了 简化 整个 过 程 ， 我 们 首先 
定义 代价 函数 类 型 ， 并 且 定 义 Create 成 员 来 使 用 Ceres 当 中 的 AutoDi ff 特 
性 : 


kÀ slambook/ch10/ceres_custombundle/ SnavelyReprojectionError.h 


1 |class SnavelyReprojectionError 

WE 

3 | public: 

4 SnavelyReprojectionError(double observation_x, double observation_y) :observed_x 
(observation_x) ,observed_y (observation_y) {} 

5 

6 template<typename T> 

7 bool operator() (const T* const camera, const T* const point, T* residuals) const 

8 { 

9 // cameral0,1,2] are the angle-azis rotation 

10 T predictions [2] ; 

11 CamProjectionWithDistortion(camera, point, predictions) ; 

12 residuals[0] = predictions[0] - T(observed_x); 

13 residuals[1] = predictions[1] - T(observed_y) ; 

14 

15 return true; 

16 } 

17 

18 static ceres::CostFunction* Create(const double observed_x, const double 
observed_y){ 

19 return (new ceres: :AutoDiffCostFunction<SnavelyReprojectionError ,2,9,3>( 

20 new SnavelyReprojectionError(observed_x,observed_y))); 

21 } 

22 

23 | private: 

24 double observed_x; 

25 double observed_y; 

2 |}; 


定义 好 之 后 ， 我 们 就 可 以 使 用 SnavelyReprojectionError::Create 图 数 来 
轻松 地 构建 这 个 目标 函数 : 


kà slambook/ch10/ceres_custombundle/ceresBundle.cpp (片段 ) 


1 | void BuildProblem(BALProblem* bal_problem, Problem* problem, const BundleParams& 


params) 


2 |{ 


3 const int point_block_size = bal_problem->point_block_size(); 


4 const int camera_block_size = bal_problem->camera_block_size() ; 
5 double* points = bal_problem->mutable_points() ; 

6 double* cameras = bal_problem->mutable_cameras() ; 

7 

8 // Observations is 2 * num_observations long array observations 
9 // [u_1, v2, ... u_n], where each u_i is two dimensional, the z 
10 // and y position of the observation. 


11 const double* observations = bal_problem->observations () ; 


13 for(int i = 0; i < bal_problem->num_observations(); ++i){ 


14 CostFunction* cost_function; 


16 // Each Residual block takes a point and a camera as input 


17 // and outputs a 2 dimensional Residual 


19 cost_function = SnavelyReprojectionError::Create(observations[2*i + 0], 


observations [2*i + 1]); 


21 // If enabled use Huber's loss function. 

2 LossFunction* loss_function = params.robustify ? new HuberLoss(1.0) : NULL; 

23 

24 // Each observatoin corresponds to a pair of a camera and a point 

25 // which are identified by camera_index() [i] and point_index() [i] 

26 // respectively. 

27 double* camera = cameras + camera_block_size * bal_problem->camera_index() [i]; 
28 double* point = points + point_block_size * bal_problem->point_index() [i]; 

29 

30 problem->AddResidualBlock(cost_function, loss_function, camera, point); 


值得 一 提 的 是 ， 为 了 使 用 Schur 消 元 ，Ceres 采 取 的 策略 和 g2o 有 很 大 
的 不 同 。 Ceres 采 用 额外 的 类 型 ParameterBlockOrdering 来 管理 Schur 消 元 顺 
序 ， 并 且 使 用 AddFlement-ToGroup 来 对 变量 进行 编号 从 而 定义 消 元 顺 
序 。 例 如 下 面 设置 点 云 变 量 为 0， 相 机 变量 为 1 就 可 以 让 点 云 变量 先进 行 
消 元 (优先 消 元 编号 最 小 的 变量 ) : 


由 slambook/ch10/ceres_custombundle/ceresBundle.cpp 


稠密 或 者 稀 下 的 


void SetOrdering(BALProblem* bal_problem, ceres::Solver::Options* options, const 
BundleParams& params) 
{ 
const int num_points = bal_problem->num_points() ; 
const int point_block_size = bal_problem->point_block_size() ; 
double* points = bal_problem->mutable_points() ; 
7 const int num_cameras = bal_problem->num_cameras() ; 
8 const int camera_block_size = bal_problem->camera_block_size() ; 
9 double* cameras = bal_problem->mutable_cameras() ; 
10 
11 if (params.ordering == "automatic") 
12 return; 
13 
14 ceres::ParameterBlockOrdering* ordering = new ceres::ParameterBlock0rdering; 
15 
16 // The points come before the cameras 
7 for(int i = 0; i < num_points; ++i) 
18 ordering->AddElementToGroup(points + point_block_size * i, 0); 
19 
20 for(int i = 0; i < num_cameras; ++i) 
21 ordering->AddElementToGroup(cameras + camera_block_size * i, 1); 
22 
23 options->linear_solver_ordering.reset (ordering) ; 
24 ik 


10.4.2 ”求解 


Ceres 除 了 能 够 在 原始 数组 上 操作 以 外 ， 另 一 大 优势 在 于 它 的 优化 设 
置 非常 便捷 。 er UL Misc 以 及 选择 
给 Solver::Options 的 类 型 成 
类 型 几乎 
SALT Cees ORV RAE A 入 置 和 管理 ， 包 括 我 们 上 面 提 到 的 Schur; 消 元 
顺序 《注意 上 面 国 数 当 中 的 最 后 一 行 ) 。 


员 变 量 进 和 于 赋值 来 调整 ， 这 种 方式 比 g20 快 捷 便利 很 多 。 Options 类 


内 slambook/ch10/ceres_custombundle/ceresBundle.cpp (片段 ) 


1 | void SetSolverOptionsFromFlags(BALProblem* bal_problem, 


2 | const BundleParams& params, Solver::Options* options) { 


4 options->max_num_iterations = params.num_iterations; 

5 options->minimizer_progress_to_stdout = true; 

6 options->num_threads = params.num_threads; // 用 于 计算 的 线程 数目 ， 可 以 加 速 雅 可 
rt 4B ME ay i+ E 


8 // 下 降 策 略 的 选取 


9 CHECK (StringToTrustRegionStrategyType(params.trust_region_strategy, 

10 koptions->trust_region_strategy_type)) ; 

11 

12 // linear solver 的 选取 

13 CHECK (ceres: :StringToLinearSolverType(params.linear_solver, &options-> 


linear_solver_type)); 


14 CHECK (ceres: :StringToSparseLinearAlgebraLibraryType (params. 
sparse_linear_algebra_library, &options->sparse_linear_algebra_library_type)); 
15 CHECK (ceres: :StringToDenseLinearAlgebraLibraryType (params. 


dense_linear_algebra_library, &options->dense_linear_algebra_library_type)); 


17 // 变量 排序 的 设置 
18 SetOrdering(bal_problem, options ,params) ; 
19 |} 


Ceres 的 求解 也 很 简单 。 和 前 面 提 到 的 曲线 拟 合 的 例子 一 样 ， 只 需 
简单 地 设置 一 下 关于 梯度 和 相 邻 两 次 迭代 之 间 目 标 遂 数 之 差 的 相关 阅 值 
就 可 以 。Solve 函 数 负责 Ceres 的 求解 功能 ， 只 需要 传 给 它 对 应 的 选项 、 目 
标 图 数 即 可 。Summary 类 型 用 来 负责 函数 求解 每 一 次 迭代 的 结果 统计 。 


内 slambook/ch10/ceres_custombundle/ceresBundle.cpp (片段 ) 


1 options.gradient_tolerance = 1e-16; 


2 options.function_tolerance = 1e-16; 
3 Solver: :Summary summary; 
4 Solve(options, &problem, &summary) ; 


和 8g2o 示 例 一 样 ， 该 程序 在 运行 后 也 可 以 在 执行 文件 目录 下 找到 fi 
nal.ply 文 件 和 initial.ply， 由 于 该 程序 和 8g2o 程 序 采用 一 致 的 数据 ， 因 此 


initial.ply 具 有 相同 的 图 案 。 使 用 MeshLab 软 件 打开 fi nal.ply 可 以 看 到 如 图 
10-15 所 示 的 结果 ， 通 过 对 比 也 可 以 发 现 优化 结果 和 g2o 完 全 一 样 。 


图 10-15 ”Ceres 的 优化 结果 。 


Ceres 库 公开 的 API 说 明 详细 ， 同 时 源 代码 可 读 性 也 高 ， 推 荐 读者 多 
多 阅读 Ceres 源 代码 ， 并 且 自 己 党 试 在 Schur 消 元 操作 中 只 消去 部 分 点 云 变 
量 ， 或 者 夹杂 着 消去 一 些 相 机 变量 。 这 只 需要 操作 
ceres::ParameterBlockOrdering::AddElementIToGroup 国 数 ， 在 对 应 变量 地 址 
上 ， 用 序号 指定 顺序 即 可 。 相 比 g2o 这 样 公开 文档 太 少 的 库 ， 我 们 更 加 推 
荐 读者 更 多 地 使 用 Ceres 这 样 的 库 来 做 SLAM。 


10.5 小结 


本 讲 比较 深入 地 探讨 了 状态 估计 间 题 与 图 优化 的 求解 。 我 们 看 到 在 
经 典 模型 中 SLAM 可 以 看 成 状态 估计 问题 。 如 果 我 们 假设 马尔 可 夫 性 ， 只 
考虑 当前 状态 ， 则 得 到 以 EKF 为 代表 的 滤波 器 模型 。 如 若 不 然 ， 我 们 也 
可 以 选择 考虑 所 有 的 运动 和 观测 ， 它 们 构成 一 个 最 小 二 乘 问题 。 在 只 
观测 方程 的 情况 下 ， 这 个 问题 称 为 BA， 并 可 利用 非 线 性 优化 方法 求解 。 
我 们 仔细 讨论 了 求解 过 程 中 的 稀 臣 性 问题 ， 指 出 了 该 问题 与 图 优化 之 间 


的 联系 。 最 后 ， 我 们 演示 了 如 何 使 用 g2o 和 Ceres 库 求解 同一 个 优化 问题 ， 
让 读者 对 BA 有 一 个 直观 的 认识 。 


习题 
1. 证 明 式 (10.25) 成 立 。 提 示 : 你 可 能 会 用 到 SMW ( Sherman- 
Morrison-Woodbury) 公式 ， 参 考 文献 [76,6]。 


2. 对 比 g20 和 Ceres 的 优化 后 目标 水 数 的 数值 。 指 出 为 什么 两 者 在 
Meshlab 中 效果 一 样 但 数值 却 不 同 。 


3. 对 Ceres 当 中 的 部 分 点 云 进行 Schur 消 元 ， 看 看 结果 会 有 什么 区 别 。 
4. 证 明 S FE R47 EE FEBS. 


5. 阅 读 文献 [28]， 看 看 g20 对 核 水 数 是 如 何 处 理 的 。 与 Ceres 中 的 Loss 
eee 何 联系 ? 


6.* 在 两 个 示例 中 ， 我 们 优化 了 相机 位 姿 、 以 fk k, 为 参数 的 相机 内 


参 及 路 标点 。 请 考虑 使 用 第 5 讲 介绍 的 完整 的 相机 模型 进行 优化 ， 即 ， 至 
DZER f Pi PK k, 这些 量 。 修 改 现在 的 Ceres 和 g20 程 序 以 完成 实 


验 。 


[1] 这 里 的 等 号 并 不 严格 ， 实 际 允 许 相 差 与 x , 无 关 的 常数 。 


[2] 这 里 看 似 有 一 点 儿 循 环 定义 的 意思 。 我 们 由 >P , 定义 了 K ， 又 把 ”7P , 写成 了 K 的 表达 式 。 然 
而 ， 实 际 当 中 K 可 以 不 依靠 P , 算得 ， 参 见 本 讲 的 习题 。 


[3] 粒子 滤波 器 的 原理 与 卡尔 曼 滤 波 有 较 大 不 同 。 


[4] 也 译 为 光束 法 平 差 、 捆 集 调整 等 ， 但 笔者 觉得 没有 Bundle Adjustment 这 个 英文 来 得 直观 ， 所 以 
这 里 保留 英文 名 称 。 


[5] 所 谓 邻 接 矩 阵 是 这 样 一 种 矩阵 ， 它 的 第 i 个 元 素描 述 了 节点 i 和 j 是 否 存 在 一 条 边 。 如 果 存 在 
此 边 ， 设 这 个 元 素 为 1， 否 则 设 为 0。 


[6] 这 种 限制 实在 让 人 很 愧 恼 。 


第 11 讲 ”后 端 2 


主要 目标 

1. 理 解 Pose Graph 优化 。 

2. 理 解 因 子 图 优化 。 

3. 理 解 增 量 式 图 优化 的 工作 原理 。 

4. 通 过 实验 掌握 g2o 的 Pose Graph 优化 与 gtsam 的 因子 图 优化 。 


上 一 讲 我 们 重点 介绍 了 以 BA 为 主 的 图 优化 。BA 能 精确 地 优化 每 个 相 
机 位 姿 与 特征 点 位 置 。 不 过 在 更 大 的 场景 中 ， 大 量 特征 点 的 存在 会 严重 
降低 计算 效率 ， 导 致 计算 量 越 来 越 大 以 至 于 无 法 实时 化 。 本 讲 介绍 两 种 
在 更 大 场景 下 使 用 的 后 端 优化 方法 : 位 次 图 和 因子 图 。 


11.1 位 姿 图 (PoseGraph) 


11.1.1 Pose Graph 的 意义 


带 有 相机 位 次 和 空间 点 的 图 优化 称 为 BA， 能 够 有 效 地 求解 大 规模 的 
定位 与 建 图 问题 。 但 是 ， 随 着 时 间 的 流逝 ， 机 器 人 的 运动 轨迹 将 越 来 越 
长 ， 地 图 规模 也 将 不 断 增长 。 像 BA 这 样 的 方法 ， 计 算 效率 会 〈 令 人 担忧 
地 ) 不 断 下 降 。 根 据 前 面 的 讨论 ， 我 们 发 现 特征 点 在 优化 问题 中 占据 了 
绝 大 部 分 。 而 实际 上 ， 经 过 若干 次 观测 之 后 ， 那 些 收敛 的 特征 点 ， 空 间 
位 置 估计 会 收 合 至 一 个 值 保持 不 动 ， 而 发 散 的 外 点 则 通常 看 不 到 了 。 对 
收敛 点 再 进行 优化 ， 似 乎 是 有 些 费力 不 讨好 的 。 因 此 ， 我 们 更 倾向 于 在 
优化 几 次 之 后 就 把 特征 点 固定 住 ， 只 把 它们 看 作 位 姿 估 计 的 约束 ， 而 不 
再 实际 地 优化 它们 的 位 置 估 计 。 


沿 着 这 个 思路 往 下 走 ， 我 们 会 想到 : 是 否 能 够 完全 不 管 路 标 而 只 管 
轨迹 呢 ? 我 们 完全 可 以 构建 一 个 只 有 轨迹 的 图 优化 ， 而 位 姿 节点 之 间 的 
边 ， 可 以 由 两 个 天 键 帧 之 间 通 过 特征 匹配 之 后 得 到 的 运动 估计 来 给 定 初 
始 值 。 不 同 的 是 ， 一 旦 初始 估计 完成 ， 我 们 就 不 再 优化 那些 路 标点 的 位 


置 ， 而 只 关心 所 有 的 相机 位 姿 之 间 的 联系 了 。 通 过 这 种 方式 ， 我 们 省 去 
了 大 量 的 特征 点 优化 的 计算 ， 只 保留 了 关键 帧 的 轨迹 ， 从 而 构建 了 所 谓 
的 位 姿 图 (Pose Graph) ， 如 图 11-1 所 示 。 
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Bundle Adjustment Pose Graph 


图 11-1 Pose Graph 示意 图 。 当 我 们 不 再 优化 Bundle Adjustment 中 的 路 标点 ， 仅 把 它们 看 成 
对 姿态 节点 的 约束 时 ， 就 得 到 了 一 个 计算 规模 减 小 很 多 的 Pose Grapho 


我 们 知道 ， 在 BA 中 特征 点 数量 远大 于 位 姿 节点 。 一 个 关键 帧 往往 关 
联 了 数 百 个 关键 点 ， 而 实时 BA 的 最 大 计算 规模 ， 即 使 利用 稀 疏 性 ， 在 当 
前 的 主流 CPU 上 一 般 也 就 是 几 万 个 点 左右 。 这 就 限制 了 SLAM 应 用 场景 。 
所 以 ， 当 机 器 人 在 更 大 东 围 的 时 间 和 空间 中 运动 时 ， 必 须 考虑 一 些 解决 
方式 : 要 么 像 滑 动 窗口 法 那样 ， 丢 乔 一 些 历 史 效 据 "7 ;要 么 像 Pose Graph 
的 做 法 那样 ， 舍 弃 对 路 标点 的 优化 ， 只 保留 Pose 之 间 的 边 ， 使 用 Pose 


Graph!”®79.80] ë 
11.1.2 Pose Graph 的 优化 


那么 ，Pose Graph 图 优化 中 的 节点 和 边 都 是 什么 意思 呢 ? 这 里 的 节点 
表示 相机 位 资 ， 以 5 所 来 表达 。 而 边 ， 则 是 两 个 位 姿 节 点 之 间 相 对 运 


动 的 估计 ， 该 估计 可 能 来 自 于 特征 点 法 或 直接 法 ， 但 不 管 如 何 ， 我 们 估 
计 了 ， 上 比如 说 & ME 之 间 的 一 个 运动 AE, 。 该 运动 可 以 有 若干 种 表达 方 


式 ， 我 们 取 比 较 自然 的 一 种 : 
Agi; = &;* o &; = In (exp ((—&:)") exp (€9)) ， (11.1) 
或 按 李 和 群 的 写法 : 


AT;; = T+T). (11.2) 


按照 图 优化 的 思路 来 看 ， 实 际 当中 该 等 式 不 会 精确 地 成 立 ， 因 此 我 
们 设立 最 小 二 乘 误差 ， 然 后 和 以 往 一 样 ， 讨 论 误 差 关 于 优化 变量 的 导 
数 。 这 里 ， 我 们 把 上 式 的 AT, 移 至 等 式 右 侧 ， 构 建 误差 e : 


ey ,AT DY) 
= In (exp((—&i;)) exp((—éi)") exp(€)) ”. 
注意 优化 变量 有 两 个 : 5 和 5 ， 因 此 我 们 求 e; 关于 这 两 个 变量 的 导 


数 。 按 照 李 代数 的 求 导 方式 ， 给 5 和 5 各 一 个 左 扰动 : 65 MOE. FER 


(11.3) 


6ij = In (TT exp((—6;)") exp(5€)T;)~ . (11.4) 


该 式 中 ， 两 个 扰动 项 被 夹 在 了 中 间 。 为 了 利用 BCH 近 似 ， 我 们 和 希望 
把 扰动 项 移 至 式 子 左 侧 或 右 侧 。 回 忆 第 4 讲习 题 中 的 伴随 性 质 ， 即 式 
(4.49)。 如 果 你 没有 做 过 这 个 习题 ， 那 就 暂时 想当然 地 认为 其 正确 。 


exp ((Ad(T)é)’) = T exp(€*)T*. (11.5) 


稍 加 改变 ， 有 : 
exp(£^)T = T exp ((Aa(z~1)€)") l (11.6) 


该 式 表 明 ， 通 过 引入 一 个 伴随 项 ， 我 们 能 够 “交换 ”扰动 项 左右 侧 的 了 
。 利 用 它 ， 可 以 将 扰动 挪 到 最 右 (当然 最 左 亦 可 ) ， 导 出 右 乘 形式 的 雅 
FILE ABBE 〈 挪 到 左边 时 形成 左 乘 ) : 


6j = In (T3 T7" exp((—d€;)) exp(5€/)T}) ” 
= In (TT "7; exp ((—Ad(T; ")66:)”) exp((Ad(T; 1)6€;)") 
~ In (T3 'T7 T; [I — (Ad(T"1)6€;)* + (Ad(T1)5€;)*]) ° 


0e;; 0e;; 
X eij + ADS 二 ae 96; 


因此 ， 按 照 李 代数 上 的 求 导 法 则 ， 我 们 求 出 了 误差 天 于 两 个 位 姿 的 
雅 可 比 矩 阵 。 关 于 TT 的 : 


deij 


— a gly. as 
005; =-J, (eij)Ad(T; ). ee 
以 及 关于 TT 的: 
dei; = J7 (e;;)Ad(T7! (11.9) 
ðe ~" (eij)Ad(T; 小 l 


如 果 读者 觉得 这 部 分 求 导 理 解 起 来 有 困难 ， 可 以 回 到 第 4 讲 瘟 习 一 下 
李 代 数 部 分 的 内 容 。 不 过 ， 前 面 也 说 过 ， 由 于 se(3) 上 的 左右 雅 可 比 J AZ 
式 过 于 复杂 ， 我 们 通常 取 它 们 的 近似 。 如 果 误 差 接 近 于 零 ， 我 们 就 可 以 
设 它 们 近似 为 1 或 


1| pe pe 
Tr (€ij)® I+ 了 | Re P | (11.10) 
0 oè 


理论 上 讲 ， 即 使 在 优化 之 后 ， 由 于 每 条 边 给 定 的 观测 数据 并 不 一 
致 ， 误 差 通 音 也 不 见得 近似 于 零 ， 所 以 简单 地 把 这 里 的 ) 设置 为 [会 有 一 
定 的 损失 。 稍 后 我 们 将 通过 实践 来 看 看 理论 上 的 区 别 是 否 明显 。 


了 解 雅 可 比 求 导 后 ， 剩 下 的 部 分 融和 普通 的 图 优化 一 样 了 。 简 而 言 
之 ， 所 有 的 位 姿 顶 点 和 位 姿 一 一 位 姿 边 构成 了 一 个 图 优化 ， 本 质 上 是 一 
个 最 小 二 乘 问题 ， 优 化 变量 为 各 个 顶点 的 位 姿 ， 边 来 自 于 位 姿 观 测 约 
束 。 Ide 为 所 有 边 的 集合 ， PBA A BARA 


min ; 2 el Dj; eij. (11.11) 

我 们 依然 可 以 用 高 斯 牛顿 法 、 列 文 伯 格 一 马 夸 尔 特 方法 等 求解 此 问 
题 ， 除 了 用 李 代 数 表 示 优 化 位 资 以 外 ， 别 的 都 是 相似 的 。 根 据 先前 的 经 
验 ， 这 上 自然 可 以 用 Ceres 或 g2o 进 行 求解 。 我 们 不 再 讨论 优化 的 详细 过 程 ， 
上 一 讲 已 经 讲 得 够 多 了 。 


11.2 SCH: 位 姿 图 优化 
11.2.1 ”g20o 原 生 位 姿 图 


下 面 演示 使 用 g20 进 行 位 姿 图 优化 。 首 先 ， 请 读者 用 g2o_viewer 打 开 
我 们 预先 生成 的 仿真 位 姿 图， 位 于 slambook/ch1l1/sphere.g20 中 ， 如 图 11-2 
所 示 。 


Odometry 


Loop Closure 


图 11-2”g20 仿 真 产 生 的 位 姿 图 。 真 值 是 完整 的 球形 ， 在 真 值 上 添加 噪声 后 得 到 带 累计 误差 
的 仿真 数据 。 


该 位 姿 图 是 由 g2o 自 带 的 create sphere 程 序 仿真 生成 的 。 它 的 真实 轨迹 
为 一 个 球 ， 由 从 下 往 上 的 多 个 层 组 成 。 每 层 为 一 个 正 圆 形 ， 很 多 个 大 小 
不 一 的 圆 形 层 组 成 了 一 个 完整 的 球体 ， 共 包含 2500 个 位 姿 节点 (图 11-2 左 
+) ， 可 以 看 成 一 个 转圈 上 升 的 过 程 。 然 后 ， 仿 真 程序 生成 了 t- 1 到 t 时 
刻 的 边 ， 称 为 odometry 边 (里 程 计 ) 。 此 外 ， 又 生成 层 与 层 之 间 的 边 ， 称 
为 loop closure (回环 ， 将 在 下 一 讲 详细 介绍 ) 。 随 后 ， 在 每 条 边 上 添加 
观测 噪声 ， 并 根据 里 程 计 边 的 噪声 ， 重 新 设置 节点 的 初始 值 。 这 样 ， 就 
得 到 了 带 累积 误差 的 位 姿 图 数据 (图 11-2 右 下 ) 。 它 局 部 看 起 来 像 球 体 的 
一 部 分 ， 但 整体 形状 与 球体 相差 甚 远 。 现 在 我 们 从 这 些 带 噪声 的 边 和 节 
点 初始 值 出 发 ， 尝 试 优化 整个 位 姿 图 ， 得 到 近似 真 值 的 数据 。 


当然 ， 实 际 当中 的 机 器 人 肯定 不 会 出 现 这 样 正 球形 的 运动 轨迹 ， 以 
及 如 此 完整 的 里 程 计 与 回环 观测 数据 。 仿 真 成 正 球 的 好 处 是 我 们 能 够 直 
观 地 看 到 优化 结果 是 否 正确 〈 只 要 看 它 各 个 角度 圆 不 圆 就 行 了 ) 。 读 者 
可 以 单 击 g2o_viewer 中 的 optimize 国 数 ， 看 到 每 步 的 优化 结果 和 收敛 的 过 
程 。 另 一 方面 ，sphere.g2o 也 是 一 个 文本 文件 ， 可 以 用 文本 编辑 器 打开 ， 
查看 它 里 面 的 内 容 。 文 件 前 半 部 分 由 节点 组 成 ， 后 半 部 分 则 是 边 : 


1 | VERTEX_SE3:QUAT 0 -0.125664 -1.53894e-17 99.9999 0.706662 4.32706e-17 0.707551 
-4.3325e-17 

3 | EDGE_SE3:QUAT 1524 1574 -0.210399 -0.0101193 -6.28806 -0.00122939 0.0375067 
-2.85291e-05 0.999296 10000 0 0 0 0 O 10000 0 O 0 O 10000 O O O 40000 O 0 40000 0 
40000 


可 以 看 到 ， 节 点 类 型 是 VERTEX_SE3， 表 达 一 个 相机 位 姿 。g2o 默 认 
使 用 四 元 数 和 平移 向 量 表达 位 姿 ， 所 以 后 面 的 字段 意义 为 : ID, tt, 
,qq, Tyo 前 3 个 为 平移 向 量 元 素 ， 后 4 个 为 表示 旋转 的 单位 四 元 数 。 
同样 ， 边 的 信息 为 两 个 节点 的 ID，t ,t,t, qoll” EREA E 


角 〈 由 于 信息 矩阵 为 对 称 阵 ， 只 需 保 存 一 半 即 可 ) 。 可 以 看 到 这 里 把 信 
息 和 矩阵 设 成 了 对 角 阵 。 


元 数 表示 的 。 由 于 仿真 数据 也 是 g20 生 成 的 ， 所 以 用 g2o 本 身 优化 就 无 须 
我 们 多 做 什么 工作 了 ， 只 需 配 置 一 下 优化 参数 即 可 。 程 序 


slambook/ch11/pose_graph_g20_SE3.cpp 演 示 了 如 何 使 用 列 文 伯 格 一 马 硅 尔 
特 方 法 对 该 位 姿 图 进行 优化 ， 并 把 结果 存储 至 result.g20 文 件 中 。 


内 slambook/ch11/pose_graph_g20_SE3.cpp 


1 |#include <iostream> 
2 |#include <fstream> 


3 |#include <string> 


5 |#include <g2o/types/slam3d/types_slam3d.h> 

6 | #include <g20/core/block_solver.h> 

7 |#include <g2o0/core/optimization_algorithm_levenberg.h> 

s |#include <g2o0/core/optimization_algorithm_gauss_newton.h> 
9 | #include <g20/solvers/dense/linear_solver_dense.h> 

10 | #include <g20/solvers/cholmod/linear_solver_cholmod.h> 


1 |using namespace std; 


13 | FEE REE GOOG EI GGG IIT I IIT III 
u | * 本 程序 演示 如 何 用 g20 solver 进行 位 姿 图 优化 
is |* sDhere.g2o 是 人 工 生 成 的 一 个 Pose graph ， 我 们 来 优化 它 。 


* 尽管 可 以 直接 通过 load 函数 读 取 整 个 图 ， 但 我 们 还 是 自己 来 实现 读 取代 码 ， 以 期 获得 更 深 
刻 的 理解 。 
* 这 里 使 用 g20/types/slam3d/ 中 的 SEZ 表示 位 次 它 实 质 上 是 四 元 数 而 非 李 代数 。 


* SGI GIGI GICIGI GGG ICICI I i tk /- 


int main( int argc, char** argv ) 


£ 


if ( argc != 2 ) 


t 
cout<<"Usage: pose_graph_g2o0_SE3 sphere.g2o"<<end1; 
return 1; 

} 

ifstream fin( argv[1] ); 

if ( !fini 9 

{ 
cout<<"file "<<argv[1]<<" does not exist."<<endl; 
return 1; 

} 


typedef g20::BlockSolver<g20::BlockSolverTraits<6,6>> Block; // 6x26 
BlockSolver 


Block: :LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<Block:: 


PoseMatrixType>() ; 

Block* solver_ptr = new Block( linearSolver ); // 和 矩阵 块 求解 器 
g2o0::OptimizationAlgorithmLevenberg* solver=new g2o:: 
OptimizationAlgorithmLevenberg(solver_ptr) ; 

g20::SparseOptimizer optimizer; // 图 模型 
optimizer.setAlgorithm( solver ); // 设置 求解 器 


int vertexCnt = 0, edgeCnt = 0; // 顶点 和 边 的 数量 
while ( !fin.eof() ) 
{ 
string name; 
fin>>name; 
if ( name == "VERTEX_SE3:QUAT" ) 
{ 
// SE3 顶点 
g2o::VertexSE3* v = new g2o: :VertexSE3() ; 
int index = 0; 
fin>>index; 
v->setId( index ); 
v->read(fin); 
optimizer .addVertex(v) ; 
vertexCnt++; 
if ( index==0 ) 


57 v->setFixed(true) ; 

58 } 

59 else if ( name=="EDGE_SE3:QUAT" ) 

60 { 

61 // SE3-SE3 id 

62 g20::EdgeSE3* e = new g20::EdgeSE3() ; 

63 int idx1, idx2; // 关联 的 两 个 顶点 

64 fin>>idx1>>idx2; 

65 e->setId( edgeCntt+ ); 

66 e->setVertex( 0, optimizer.vertices() [idx1] ); 
67 e->setVertex( 1, optimizer.vertices()[idx2] ); 
68 e->read(fin) ; 

69 optimizer .addEdge(e) ; 

70 } 

71 if ( !fin.good() ) break; 

72 } 

73 

74 cout<<"read total "<<vertexCnt<<" vertices, "<<edgeCnt<<" edges."<<endl; 
75 

76 cout<<"prepare optimizing ..."<<endl; 

77 optimizer .setVerbose (true) ; 

78 optimizer.initialize0ptimization(); 

79 cout<<"calling optimizing ..."<<endl; 

80 optimizer.optimize(30) ; 

81 

82 cout<<"saving optimization results ..."<<endl; 

83 optimizer.save("result.g2o0") ; 

84 

85 return 0; 

s |} 


我 们 选择 了 6x 6 的 块 求解 器 ， 使 用 列 文 但 格 一 马 夸 尔 特 下 降 方 式 ， 
迭代 次 数 选择 30 次 。 调 用 此 程序 对 位 次 图 进行 优化 : 


$ build/pose_graph_g20_SE3 sphere.g2o 

read total 2500 vertices, 9799 edges. 
prepare optimizing ... 

calling optimizing ... 

schur= 0 lambda= 805.622433 levenbergIter= 1 


schur= 0 lambda= 537.081622 levenbergIter= 1 


schur= 0 lambda= 358.054415 levenbergIter= 1 


schur= 0 lambda= 238.702943 levenbergIter= 1 


schur= 0 lambda= 159.135295 levenbergIter= 1 


0 lambda= 0.003127 levenbergIter= 1 
iteration= 29 chi2= 44811.248504 time= 1.76326 
0 lambda= 0.003785 levenbergIter= 2 


saving optimization results ... 


iteration= 0 chi2= 1023011093.967638 time= 0.851879 cumTime= 0.851879 edges= 9799 


iteration= 1 chi2= 385118688.233188 time= 0.863567 cumTime= 1.71545 edges= 9799 


iteration= 2 chi2= 166223726.693659 time= 0.861235 cumTime= 2.57668 edges= 9799 


iteration= 3 chi2= 86610874.269316  time= 0.844105 cumTime= 3.42079 edges= 9799 


iteration= 4 chi2= 40582782.710190 time= 0.862221 cumTime= 4.28301 edges= 9799 


iteration= 28 chi2= 45095.174398 time= 0.869451 cumTime= 30.0809 edges= 9799 schur= 


cumTime= 31.8442 edges= 9799 schur= 


然后 ， 用 g2o_viewer 打 开 result.g2o 查 看 结果 ， 如 图 11-3 所 示 。 
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Spanning Tree 


Initial Guess 
SetZero 
Optimize loaded result.g2o with 2500 vertices and 9799 measurements 
Qui graph is fixed by priors or nodes are already fixed 
uit 


图 11-3 ”使 用 g2o 自 带 的 顶点 与 边 求解 的 结果 。 


结果 从 一 个 不 规则 的 形状 优化 成 了 一 个 看 起 来 完整 的 球 。 这 个 过 程 
实质 上 和 我 们 单 击 g2o_viewer 上 的 Optimize 按 钮 没有 区 别 。 下 面 ， 我 们 根 
据 前 面 的 李 代 数 推导 来 实现 一 下 李 代 数 上 的 优化 。 


11.2.2” 李 代数 上 的 位 姿 图 优化 

还 记得 我 们 用 Sophus 来 表达 李 代 数 的 事情 吗 ? 我 们 来 试 试 把 Sophus 
用 到 g2o 中 定义 自己 的 顶点 和 边 吧 。 

kÀ slambook/ch11/pose_graph_g2o_lie_algebra.cpp (片段 ) 


42 


43 


#include <iostream> 
#include <fstream> 
#include <string> 


#include <Eigen/Core> 


#include <g20/core/base_vertex.h> 

#include <g20/core/base_binary_edge.h> 

#include <g20/core/block_solver.h> 

#include <g2o0/core/optimization_algorithm_levenberg.h> 
#include <g2o0/core/optimization_algorithm_gauss_newton.h> 
#include <g2o0/core/optimization_algorithm_dogleg.h> 
#include <g2o0/solvers/dense/linear_solver_dense.h> 


#include <g2o/solvers/cholmod/linear_solver_cholmod.h> 


#include <sophus/se3.h> 
#include <sophus/so3.h> 
using namespace std; 
using Sophus: :SE3; 
using Sophus: :S03; 


OSC III I III I II III I IOI II IIT ATT 

* 本 程序 演示 如 何 用 9g2o solver 进行 位 姿 图 优化 

* sphere.g20 是 人 工 生 成 的 一 个 Pose graph ， 我 们 来 优化 它 - 

* 尽管 可 以 直接 通过 load 函数 读 取 整 个 图 ， 但 我 们 还 是 自己 来 实现 读 取代 码 ， 以 期 获得 更 深 
刻 的 理解 

* 本 节 使 用 李 代 数 表 达 位 姿 图 ， 节 点 和 边 的 方式 为 自 定义 。 


* GAGGIA IG IGG IO IG GIG IG OG RR IK Ia //- 
typedef Eigen: :Matrix<double,6,6> Matrix6d; 


// 给 定 误 差 求 JR{-1} 的 近似 
Matrix6d JRInv( SE3 e ) 


{ 
Matrix6d J; 
J.block(0,0,3,3) = S03::hat(e.so3().log()); 
J.block(0,3,3,3) = S03::hat(e.translation()); 
J.block(3,0,3,3) = Eigen: :Matrix3d: :Zero(3,3); 
J.block(3,3,3,3) = S03::hat(e.so3().log()); 
J = J*0.5 + Matrix6d::Identity( ; 
return J; 

} 


// 李 代 数 顶点 
typedef Eigen::Matrix<double, 6, 1> Vector6d ; 
class VertexSE3LieAlgebra: public g2o::BaseVertex<6，SE3> 


{ 
public: 
EIGEN_MAKE_ALIGNED_OPERATOR_NEW 
bool read ( istream& is ) 
{ 
double data[7]; 
for ( int i=0; i<7; i++ ) 
is>>data[i]; 
setEstimate ( SE3 ( 
Eigen::Quaterniond ( data[6],data[3], data[4], data[5] ), 
Eigen::Vector3d ( data[0], data[1], data[2] ) 
225 


bool write ( ostream& os ) const 


$ 
68<<idQO<<" "; 
Eigen: :Quaterniond q = _estimate.unit_quaternion() ; 
os<<_estimate.translation() .transpose()<<" "; 
os<<q.coeffs() [0]<<" "<<q.coeffs() [1]<<" "<<q.coeffs() [2]<<" "<<q.coeffs() 
[3] <<end1; 
return true; 

} 


virtual void setToOriginImpl () 


{ 
_estimate = Sophus: :SE3(); 
} 
// 左 乘 更 新 
virtual void oplusImpl ( const double* update ) 
{ 
Sophus: :SE3 up ( 
Sophus::S03 ( update[3], update[4], update[5] ), 
Eigen::Vector3d ( update[0], update[1], update[2] ) 
); 
_estimate = up*_estimate; 


Js 


// 两 个 李 代数 节点 之 边 
class EdgeSE3LieAlgebra: public g2o::BaseBinaryEdge<6, SE3, VertexSE3LieAlgebra, 
VertexSE3LieAlgebra> 
{ 
public: 
EIGEN_MAKE_ALIGNED_OPERATOR_NEW 


129 


bool read ( istream& is ) 


£ 


} 


double data[7]; 
for ( int i=0; i<7; i++ ) 
is>>data[i]; 
Eigen: :Quaterniond q ( data[6], data[3], data[4], data[5] ); 
q-normalize(); 
setMeasurement ( 
Sophus::SE3 ( q, Eigen::Vector3d ( data[0], data[1], data[2] ) ) 
) ; 
for ( int i=0; i<information().rows() && is.good(); i++ ) 
for ( int j=i; j<information().cols() && is.good(); j++ ) 
{ 
is >> information() ( i,j ); 
if ( i!=j ) 
information() ( j,i ) =information() ( i,j ); 
} 


return true; 


bool write ( ostream& os ) const 


{ 


VertexSE3LieAlgebra* vi = static_cast<VertexSE3LieAlgebra*> (_vertices[0]); 
VertexSE3LieAlgebra* v2 = static_cast<VertexSE3LieAlgebra*> (_vertices[1]); 
os<<v1->id() <<" "<<v2->idQ) <<" "; 

SE3 m = _measurement; 

Eigen: :Quaterniond q = m.unit_quaternion() ; 

os<<m.translation() .transpose()<<" "; 

os<<q.coeffs() [0]<<" "<<q.coeffs() [1]<<" "<<q.coeffs() [2]<<" "<<q.coeffs() 
[3]<<" "; 

// information matrix 

for ( int i=0; i<information().rows(); i++ ) 


for ( int j=i; j<information().cols(); j++ ) 


{ 
os << information() ( i,j ) <<" "; 
} 
os<<endl; 


return true; 


// 误差 计算 与 书 中 推导 一 致 


virtual void computeError() 


{ 


Sophus::SE3 vi 
estimate () ; 
Sophus: :SE3 v2 


(static_cast<VertexSE3LieAlgebra*> (_vertices[0]))-> 


(static_cast<VertexSE3LieAlgebra*> (_vertices[1]))-> 


estimate(); 


130 _error = (_measurement.inverse()*vi.inverse()*v2) .log(); 

131 } 

132 

133 // 雅 可 比 计算 

134 virtual void linearizeOplus() 

135 { 

136 Sophus::SE3 vi = (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))-> 
estimate(); 

137 Sophus::SE3 v2 = (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))-> 
estimate(); 

138 Matrix6d J = JRInv(SE3::exp(_error)) ; 

139 // 尝试 把 J 近 似 为 T? 

140 _jacobian0plusXi = - J* v2.inverse().Adj(); 

141 _jacobianOplusXj = J*v2.inverse().AdjQ; 

142 } 

143 |}; 


AT LEM 20x Fh Fim IRN, AT HFSS HM TS read#ll write K 
MX, HEAR” Me2e0NBAHSE3IAR, 124$¢20_viewerseWwiIAIRH BR 
它 。 事 实 上 ， 除 了 内 部 使 用 Sophus 的 李 代 数 表示 之 外 ， 从 外 部 看 起 来 没 
有 什么 区 别 。 

值得 注意 的 是 这 里 雅 可 比 的 计算 过 程 。 我 们 有 若干 种 选择 : 一 是 不 
提供 雅 可 比 计 算 函 数 ， 让 g2o 自 动 计算 数值 雅 可 比 。 二 是 提供 完整 或 近似 
的 雅 可 比 计算 过 程 。 这 里 我 们 用 JRInvO 娩 数 提供 近似 的 77!。 读 者 可 以 尝 
试 把 它 近 似 为 [， 或 者 干脆 注释 掉 oplusImpl 陋 数 ， 看 看 结果 会 有 什么 区 
别 。 


之 后 调用 g2o 进 行 优 化 问题 : 


20 


我 们 发 现 ， 和 迭代 23 次 后 ， 总 体 误差 保持 不 变 ， 事 实 上 可 以 让 优化 算 
法 停止 了 。 而 上 一 个 实验 中 ， 用 满 了 30 次 迭代 后 误差 仍 在 下 降 上 。 在 调 


$ build/pose_graph_g2o_lie sphere.g2o 
read total 2500 vertices, 9799 edges. 


prepare optimizing . 
calling optimizing ... 


iteration= 0 


chi2= 781963143.389706 time= 1.9322 


schur= 0 lambda= 6706.585223 levenbergIter= 1 


iteration= 1 


chi2= 236521032.458035 time= 1.91309 


schur= 0 lambda= 2235.528408 levenbergIter= 1 


iteration= 2 


chi2= 142934798.398778 time= 1.9792 


schur= 0 lambda= 745.176136 levenbergIter= 1 


iteration= 3 


chi2= 84490229.050137 time= 2.03394 


schur= 0 lambda= 248.392045 levenbergIter= 1 


iteration= 4 chi2= 
schur= 0 lambda= 82.797348 
iteration= 21 chi2= 
schur= 0 lambda= 0.000712 


schur= 0 lambda= 0.000237 


schur= 0 lambda= 0.000079 


schur= 0 lambda= 0.000053 


schur= 0 lambda= 0.000035 


schur= 0 lambda= 0.000023 


schur= 0 lambda= 0.000031 


schur= 0 lambda= 0.000042 


schur= 0 lambda= 0.001779 


127607 . 121623 


iteration= 22 chi2= 127578. 


iteration= 23 chi2= 127578. 


iteration= 24 chi2= 127578. 


iteration= 25 chi2= 127578. 


iteration= 26 chi2= 127578. 


iteration= 27 chi2= 127578. 


iteration= 28 chi2= 127578. 


iteration= 29 chi2= 127578. 


levenbergIter= 


time= 


levenbergIter= 


889888 
levenbergIter 
158794 
levenbergIter 
157859 
levenbergIter 
157859 
levenbergIter 
157859 
levenbergIter 
157859 
levenbergIter 
157859 
levenbergIter 
157859 


levenbergIter 


time= 


time= 


time= 


time= 


time= 


time= 


time= 


time= 


saving optimization results ... 


42690811.624643 time= 2.05149 


i 


1.9686 
1 


1.94773 
=1 
1.98009 


2.04546 
=1 
1.96722 


2.11235 
= 1 
3.29151 


3.20302 
= 2 
5.56337 
= 4 


cumTime= 


cumTime= 


cumTime= 


cumTime= 


cumTime= 


cumTime= 


cumTime= 


cumTime= 


cumTime= 


cumTime= 


cumTime= 


cumTime= 


cumTime= 


cumTime= 


45. 


47. 


49. 


51 


53. 


56. 


60. 


65. 


1.9322 


3.84529 


5.8245 


7.85844 


9.90993 


43.526 


4737 


4538 


4993 


.4665 


5789 


8704 


0734 


6368 


edges= 


edges= 


edges= 


edges= 


edges= 


edges= 


edges= 


edges= 


edges= 


edges= 


edges= 


edges= 


edges= 


edges= 


9799 


9799 


9799 


9799 


9799 


9799 


9799 


9799 


9799 


9799 


9799 


9799 


9799 


9799 


用 优化 后 ， 查 看 result_lie.g2o 观 察 它 的 结果 ， 如 图 11-4 所 示 。 从 肉眼 上 看 
不 出 任何 区 别 。 
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Optimizer 
gn_var_cholmod 


Parameters 


Spanning Tree 


Initial Guess 
SetZero 
Optimize loaded result_lie.g20 with 2500 vertices and 9799 measurements 
graph Is fixed by node 2499 
Quit 


图 11-4 ”使 用 李 代 数 自 定义 节点 与 边 优化 后 的 结果 。 


如 果 你 在 这 个 g2o_viewer 界 面 按 下 Optimize 按 钮 ，g2o 将 使 用 它 自 带 
的 SE3 顶 点 进行 优化 ， 你 可 以 在 下 方 文本 框 中 看 到 : 


loaded result_lie.g20 with 2500 vertices and 9799 measurements 

graph is fixed by node 2499 

# Using CHOLMOD poseDim -1 landMarkDim -1 blockordering 0 

Preparing (no marginalization of Landmarks) 

iteration= 0 chi2= 44360.509723 time= 0.567504 cumTime= 0.567504 edges= 9799 schur= 
0 

iteration= 1 chi2= 44360.471110 time= 0.595993 cumTime= 1.1635 edges= 9799 schur= 
0 

iteration= 2 chi2= 44360.471110 time= 0.582909 cumTime= 1.74641 edges= 9799 schur= 
0 


整体 误差 在 SE3 边 的 度量 下 为 44360， 略 小 于 之 前 30 次 迭代 时 的 
44811。 这 说 明 使 用 李 代 数 进行 优化 后 ， 我 们 在 更 少 的 和 欠 代 次 数 下 得 到 了 


更 好 的 结果 号。 
11.2.3 “小 结 


球 的 例子 是 一 个 比较 有 代表 性 的 案例 。 它 具有 和 实际 中 相似 的 里 程 
计 边 (Odometry) 和 回环 边 (LoopClosure) ， 这 也 正 是 实际 SLAM 中 一 
个 位 姿 图 中 可 能 有 的 东西 。 同 时 ,“ 球 ”也 具有 一 定 的 计算 规模 : 它 总 共 
有 2,500 个 位 姿 节 点 和 近 10,000 条 边 ， 我 们 发 现 优化 它 费 了 不 少时 间 (48 
对 于 实时 性 要 求 很 强 的 前 端 来 说 ) 。 另 一 方面 ,一般 认 为 位 姿 图 是 结构 
最 简单 的 图 之 一 。 在 我 们 不 假设 机 器 人 如 何 运动 的 前 提 下 ， 很 难 再 进 一 
步 讨 论 它 的 稀 足 性 了 因为 机 器 人 可 能 会 直线 往 前 运动 ， 形 成 带 状 的 
位 资 图， 是 稀疏 的 ;) 也 可 能 是 “左手 右手 一 个 慢 动 作 ”， 形 成 大 量 的 小 型 
回环 需要 优化 (Loopy motion) ， 从 而 变 成 像 * 球 ”那样 比较 稠密 的 位 姿 
图 。 无 论 如 何 ， 在 没有 进一步 的 信息 之 前 ， 我 们 似乎 无 法 再 利用 位 姿 图 
的 求解 结构 了 。 


APTAM" 提出 以 来 ， 人 们 就 意识 到 ， 后 端的 优化 没 必要 实时 地 响 
应 前 端的 图 像 数 据 。 人 们 倾向 于 把 前 端 和 后 端 分 开 ， 运 行 于 两 个 独立 线 
程 之 中 ， 历 史上 称 为 跟踪 (Tracking) 和 建 图 (Mapping) 一 一 虽然 如 此 
叫 ， 建 图 部 分 主要 是 指 后 端的 优化 内 容 。 通 俗 地 说 ， 前 端 需 要 实时 响应 
视频 的 速度 ， 例 如 每 秒 30 帧 ; 而 优化 可 以 慢 悠 悠 地 运行 ， 只 要 在 优化 完 
成 时 把 结果 返回 给 前 端 即 可 。 所 以 我 们 通 单 不 会 对 后 端 优化 提出 很 高 的 
速度 要 求 。 


11.3 * 因 子 图 优化 初步 
11.3.1 MHRA 


下 面 ， 我 们 从 另 一 个 角度 来 看 后 端 优化 : 所 谓 的 因子 图 
(FactorGraph) 优化 。 由 于 这 部 分 内 容 牵 涉 到 概率 图 理论 ， 超 出 了 本 书 
范围 ， 所 以 我 们 只 能 大 概 地 介绍 如 何 从 概率 图 角度 来 看 待 这 个 问题 。 如 
果 读 者 对 概率 图 模型 感 兴趣 ， 建 议 阅读 文献 [82]。 或 者 ， 如 果 对 如 何 使 用 
因子 图 实现 SLAM 后 端 优化 的 细节 感 兴趣 ， 可 以 阅读 文献 [83,84,85,86]。o 


从 贝 叶 斯 网 络 (Bayes Network) 的 角度 来 看 ，SLAM 可 以 自然 地 表 
达成 一 个 动态 贝 叶 斯 网 络 (Dynamic Bayes Network, DBN) 。 与 图 优化 
类 似 ， 贝 叶 斯 网 络 是 一 种 概率 图 ， 由 随机 变量 的 节点 和 表达 随机 变量 条 
件 独立 性 的 边 组 成 ， 形 成 一 个 有 向 无 环 图 (Directed Acyclic Graph) o 
SLAM 中 ， 由 于 我 们 有 运动 方程 和 观测 方程 ， 它 们 恰好 表示 了 状态 变量 之 
间 的 条 件 概 率 ， 如 图 11-5 所 示 。 


oe ara 


图 11-5 “以 贝 叶 斯 网 络 形式 表达 的 SLAM 过 程 示意 图 。 竖 线 为 运动 方程 ， 其 他 线 为 观测 方 
程 。 虚线 框 表示 一 次 观测 ， 实 线 框 表示 一 次 运动 。 


图 11-5 中 的 圆圈 表示 了 贝 叶 斯 网 络 的 节点 ， 也 就 是 与 图 相关 的 随机 变 
包括 : 

1. 相 机 位 姿 形 成 的 节点 : x , ,x ,…。 

2. 输 入 量 节点 : Uo 

3. 路 标 节点 : lo 

4. 观 测 数 据 节点 : Zo 


这 组 成 了 所 有 与 SLAM 过 程 相关 的 信息 。 另 一 方面 ， 各 线条 表示 了 它 
们 之 间 的 关系 ,箭头 表示 依赖 关系 。 例 如 ， 从 x , 指向 x , 的 箭头 ， 说 明 x ， 


依赖 于 x , ， 此 边 表 示 的 概率 是 P (x, |x , ) 一 一 事实 上 ， 运 动 方程 指出 了 从 
x, 到 x , 的 运动 关系 。 我 们 观察 实 线 框 。 这 个 框 中 ， 变 量 x , 依赖 于 x , ,u ， 
。 回 忆 运 动 方程 : 


T3 = f (x2, U3) + W3. (11.12) 


i 


它 实 际 上 给 出 了 这 几 个 变量 的 条 件 概 率 的 度量 : 
P(ax3|X2, u3). (11.13) 


同样 ， 虚 线 框 中 的 一 次 观测 ， 亦 说 明 观 测 方程 给 出 了 变量 间 的 条 件 
概率 关系 : 


P(zil|z1, 11). (11.14) 


通过 这 种 方式 ， 我 们 构建 了 一 个 贝 叶 斯 网 络 ， 它 表达 了 所 有 变量 ， 
以 及 各 个 方程 给 出 的 变量 之 间 的 条 件 概 率 关 系 。 请 注意 ， 这 只 是 说 贝 叶 
斯 网 络 表达 了 它们 ， 还 没有 说 到 贝 叶 斯 网 络 的 求解 。 事 实 上 ， 后 端 优化 
的 目标 ， 就 是 在 这 些 既 有 的 约束 之 上 ， 通 过 调整 贝 叶 斯 网 络 中 随机 变量 
的 取 值 ， 使 整个 后 验 概 率 达 到 最 大 : 


{x,l}* = arg max (xo) [[? (Tk|Ek—1, Uk) II P (zxl|zi, lj). (11.15) 


直到 现在 为 止 ， 这 和 我 们 在 图 优化 中 谈论 的 东西 并 没有 太 大 区 别 ， 
我 们 只 是 把 变量 间 的 关系 显 式 地 用 有 向 图 给 表达 出 来 而 已 。 这 样 一 个 由 
条 件 概 率 描 述 的 贝 叶 斯 网 络 ， 已 经 可 以 使 用 概率 图 模型 中 的 算法 进行 求 
解 了 。 不 过 ， 进 一 步 观察 ， 我 们 发 现 最 大 后 验 概率 由 许多 项 因子 乘积 而 
成 ， 因 此 该 贝 叶 斯 网 络 又 可 以 转化 成 一 个 因子 图 (Factor Graph) o 


11.3.2 AFA 
因子 图 是 一 种 无 向 图 ， 由 两 种 节点 组 成 : 表示 优化 变量 的 变量 节点 


， 以 及 表示 因子 的 因子 节点 。 如 果 我 们 把 图 11-5 表 示 成 因子 图 ， 就 可 画 
成 如 图 11-6 所 示 的 样子 。 


OOO 


图 11-6 ”以 因子 图 形式 表达 的 SLAM 过 程 示意 图 。 圆 圈 为 变量 节点 ， 方 块 为 因子 节点 。 


在 图 11-6 中 ， 我 们 用 圆圈 表示 变量 节点 。 注 意 与 贝 叶 斯 网 络 不 同 ， 这 
里 的 变量 是 SLAM 中 待 优化 的 部 分 ， 即 相机 位 姿 x 和 路 标 ! ， 而 没有 观测 z 
和 输入 u 一 一 因为 这 几 个 量 是 给 定 的 而 不 是 待 优化 的 。 与 之 相对 ， 因 子 节 
点 包含 了 待 优化 变量 之 间 的 关系 ， 它 们 来 自 运动 方程 和 观测 方程 。 

对 因子 图 的 优化 ， 就 是 调整 各 变量 的 值 ， 使 它们 的 因子 之 乘积 最 大 
化 一 一 它 依然 对 应 着 一 个 优化 问题 。 在 通常 的 做 法 中 ， 我 们 把 各 因子 的 
条 件 概 率 取 高 斯 分 布 的 形式 。 考 虑 到 运动 方程 为 


Lk = f (Lk—1, Uk) + We. (11.16) 


其 中 w, ~N (0,R, )。 如 果 x ， 《实际 上 x 的 取 值 还 受 其 他 因素 影 
响 ) GA, ABA: 


已 (zklzZk-1) = N (f (@x-1, Uk), Re). (11.17) 
同 理 ， 对 于 观测 数据 ， 有 : 
P (zkj\æk, lj) =N (h (ax, GAO)， (11.18) 


其 中 Q, 为 观测 方程 噪声 项 的 协 方差 


通过 假设 高 斯 分 布 ， 我 们 显 式 地 表达 了 因子 图 优化 的 目标 阔 效 。 和 
图 优化 一 样 ， 由 于 取 最 大 的 后 验 概率 相当 于 取 负 对 数 的 最 小 化 ， 所 以 因 
子 图 优化 也 对 应 着 一 个 和 第 6 讲 介绍 的 相似 的 最 小 二 乘 问题 ， 我 们 可 以 用 
高 斯 牛顿 法 或 列 文 伯 格 一 马 硅 尔 特 方法 求解 一 个 因子 图 优化 问题 。 类 似 
于 图 优化 ， 我 们 还 可 以 使 用 单元 的 、 二 元 的 或 多 元 的 因子 。 这 主要 看 它 
和 几 个 变量 节点 有 关 。 


图 11-7 是 一 个 更 实际 的 因子 图 的 例子 。 


图 11-7 更 加 实际 的 因子 图 。 添 加 了 回环 因子 (底部 中 央 ) 和 先 验 因子 (底部 两 侧 ) 。 


运动 方程 和 观测 方程 作为 因子 存在 于 图 中 。 此 外 ， 我 们 可 能 对 某 些 
相机 位 姿 具有 先 验 信息 一 一 例如 一 辆 在 室外 行驶 的 无 人 车 ， 我 们 可 能 通 
过 GPS 信 号 确定 了 轨迹 当中 某 些 节点 的 位 姿 ， 那 么 就 可 以 对 这 些 位 次 添加 
先 验 因子 。 因 为 可 能 表达 成 概率 分 布 的 信息 有 许多 种 ， 于 是 因子 图 也 可 
以 定义 许多 不 同 的 因子 ， 比 如 ， 轮 式 编码 器 的 测量 、IMU 的 测量 ， 等 
等 ， 使 之 成 为 一 种 非常 通用 的 优化 方式 。 


11.3.3 BERIE 


然而 ， 到 现在 为 止 ， 从 优化 角度 来 看 ， 优 化 一 个 因子 图 和 普通 的 图 
优化 并 没有 太 大 的 区 别 一 一 因为 我 们 最 后 面 对 的 都 是 一 个 最 小 二 乘 问 
题 ， 不 断 地 寻找 梯度 ， 使 目标 图 数 下 降 。 因 子 图 优化 的 稀 踢 性 也 与 图 优 
化 类 似 ， 我 们 可 以 通过 稀疏 QR 分 解 、Schur 补 或 Cholesky 分 解 ， 加 速 对 因 
子 图 优化 的 求解 。 所 以 我 们 不 禁 疑 惑 : 因子 图 优化 是 否 就 是 普通 图 优化 
换 了 种 说 法 呢 ? 事情 并 不 完全 是 这 样 。 


Kaess 等 人 提出 的 iSAM (incremental Smooth and Mapping) [al 中 ， 对 
因子 图 进行 了 更 加 精细 的 处 理 ， 使 得 它 可 以 增 量 式 地 处 理 后 端 优化 。 回 


忆 第 6 讲 的 内 容 ， 我 们 知道 ， 在 普通 的 图 优化 中 ， 最 终 要 计算 的 是 一 个 增 
量 陈 方 程 。 即 如 何 调整 目标 阔 效 


J(z)= >》 ebpRilevk tY eye eek (11.19) 
k k j 


中 的 优化 变量 ， 使 目标 函数 下 降 。 无 论 采 用 哪 种 梯度 下 降 策略 ， 最 
后 我 们 将 碰 到 一 个 形 如 


HAzx =g. (11.20) 


的 线性 方程 需要 求解 。 上 一 讲 ， 我 们 介绍 了 利用 该 方程 中 的 稀 下 性 
加 速 它 的 求解 过 程 。 然 而 ， 考 虑 到 图 优化 并 不 是 固定 的 ， 当 机 器 人 运动 
时 ， 新 的 节点 和 边 将 被 加 入 图 中 ， 使 得 它 的 规模 不 断 增 长 。 那 么 问题 来 
T: 是 否 每 次 新 加 一 个 节点 ， 我 们 就 要 重新 计算 一 遍 所 有 节点 的 更 新 量 
Ne? (包括 雅 可 比 的 求 取 (或 称 线性 化 ) 和 更 新 方程 的 计算 。) 


显然 ， 这 样 做 是 不 经 济 的 ， 我 们 以 图 11-8 为 例 来 解释 增 量 更 新 的 情 
况 。 这 是 一 个 位 姿 图。 当 我 们 按照 里 程 计 方式 向 其 中 添加 节点 时 ， 受 影 
响 的 节点 可 以 近似 地 看 成 只 有 最 后 一 个 与 之 相连 的 节点 ， 而 早先 节点 的 
估计 值 ， 可 以 近似 地 看 成 没有 发 生变 化 。 因 此 就 没有 必要 对 它们 进行 优 
化 。 为 什么 要 说 近似 呢 ? 因为 实际 上 新 增 节 扣 还 是 会 对 之 前 的 估计 产生 
影响 的 ， 只 是 对 最 近 的 数据 影响 最 大 ， 对 较 远 的 数据 影响 很 小 ， 可 以 忽 
略 近 。 如 果 认 为 增 量 特性 可 行 ， 那 么 这 件 事情 可 以 为 我 们 省 探 大 量 的 计 
算 一 一 至 少 我 们 不 必 在 每 次 新 增 节 点 时 ， 对 整个 图 进行 优化 。 不 过 ， 如 
果 按 照 回环 检测 的 方式 添加 节点 ， 那 么 受 影 响 的 范围 应 该 是 回环 开始 到 
当前 帧 这 一 段 中 的 所 有 节点 ， 也 就 是 整 段 轨迹 都 可 能 被 重新 调整 。 这 时 
然 加 大 了 计算 量 ， 然 而 我 们 依然 无 须 优 化 整 张 图 ， 因 为 回环 之 外 的 世 谈 
是 不 受 影响 的 。 综 上 所 述 ， 我 们 发 现 ， 在 向 图 中 添加 节点 时 ， 通 过 分 析 
受 影响 的 区 域 ， 可 以 (直观 上 ) 减少 一 些 没 必 要 的 计算 ， 加 速 后 端 优化 


流程 。 


a 


C) 新 加 的 节点 


受 影 响 的 节点 
图 11-8 增 量 更 新 示意 图 。 


在 此 思想 的 基础 上 ，Kaess 等 人 提出 的 增 量 式 因 子 图 优化 一 定 程 度 上 
解决 了 上 面 的 问题 。 从 技术 层面 上 来 看 ， 我 们 希望 在 每 次 优化 中 保存 一 
些 中 间 结 果 ; 而 当 新 的 变量 和 因子 加 入 时 ， 首 先 分 析 它 们 因子 图 之 间 的 
连接 和 影响 关系 ， 考 虑 之 前 存储 的 信息 有 哪些 可 以 继续 利用 ， 哪 些 必须 
重新 计算 ， 最 后 处 理 对 增 量 的 优化 。 话 虽 如 此 ， 由 于 具体 的 操作 步骤 需 
要 介绍 大 量 的 技术 细节 ， 且 牵涉 到 了 概率 图 理论 的 知识 ， 超 出 了 本 书 的 
讨论 范围 。 所 以 本 节 就 作为 可 选 阅读 材料 提供 给 读者 。 有 兴趣 的 读者 可 
以 阅读 iSAM、iSAM2 等 原始 论文 来 了 解 它 的 细节 处 理 。 最 后 ， 尽 管 有 增 
量 分 析 ， 但 我 们 必须 清楚 这 里 的 受 影响 节点 亦 是 近似 的 ， 所 以 在 实际 操 
作 中 ， 当 图 的 规模 发 生 一 定 程 度 的 改变 时 ， 我 们 需要 再 做 一 次 全 图 优 
化 。 


11.4 * 实 践 ; gtsam 


11.4.1 ”安装 gtsam 4.0 


下 面 ， 我 们 演示 一 个 用 因子 图 进行 位 姿 图 优化 的 例子 。 我 们 仍 使 用 
前 面 “ 球 ” 的 数据 ， 不 同 的 是 ， 这 次 用 因子 图 来 优化 它 ， 而 不 是 g20 中 的 位 
姿 图 。 我 们 将 使 用 Gtsam"*7 ， 它 是 一 个 基于 因子 图 优化 的 SLAM 后 端 库 ， 
理论 来 自 文献 [83,84]。 我 们 将 在 它 的 基础 上 ， 对 上 节 “ 球 ”的 例子 进行 优 
化 。 


1 |git clone https://bitbucket.org/gtborg/gtsam.git 


gtsam 最 新 版 本 为 4.0， 位 于 https://bitbucket.org/gtborg/gtsam。 你 可 以 
输入 以 下 命令 下 载 它 : 由 于 gtsam 比 较 大 ， 我 们 没有 在 3rdparty 文 件 夹 中 提 
供 。 与 以 往 遇 到 的 库 一 样 ，gtsam 亦 是 一 个 cmake 工 程 ， 我 们 按照 cmake 的 
方式 来 编译 安装 它 。gtsam 的 依赖 项 比较 少 ， 主 要 是 Eigen 和 tbb 库 。 如 果 
读者 跟着 本 书 一 路 走 来 ， 那 么 大 多 数 库 应 该 已 经 安装 好 了 ， 只 需 安装 tbb 
库 即 可 : 


1 | sudo apt-get install libtbb-dev 


Am. (Ficmaketp SW gtam# ha, KERB SA, gtsam 
库 比 较 大 ， 集 成 了 许多 与 因子 图 相关 的 内 容 ， 甚 至 底层 的 矩阵 、 李 代数 
运算 ， 所 以 编译 安装 会 比较 费时 ， 请 读者 耐心 等 待 。 安 装 完 成 后 ， 可 以 
在 /usr/local/include 中 找到 头 文件 ， 在 /usr/locallib 下 找到 库 文件 。gtsam 的 
库 文件 比较 简单 ， 仪 由 一 个 libgtsam.so 组 成 。 所 以 你 应 该 知道 如 何在 自己 
的 工程 中 书写 CMakeLists.txt 来 调用 gtsam 了 吧 ? 


11.4.2 ”位 姿 图 优化 


下 面 我 们 来 演示 “ 球 ” 的 例子 。 与 前 面 的 实验 一 样 ， 我 们 从 sphere.g20 
文件 中 读 取 节 点 和 边 的 信息 ， 转 换 为 因子 图 交 给 gtsam 处 理 ， 然 后 ， 把 优 
化 结果 重新 写 到 g20 文 件 ， 并 “伪装 ”成 g20 的 节点 和 边 以 便 显示 。 昌 然 这 
似乎 没有 体现 出 gtsam 的 “ 增 量 ” 特 性 ， 但 至 少 我 们 可 以 通过 这 个 例子 体验 
一 下 它 的 用 法 。 


四 slambook/ch11/pose_graph_gtsam.cpp 


#include <iostream> 
#include <fstream> 
#include <string> 
#include <Eigen/Core> 


#include <sophus/se3.h> 
#include <sophus/so3.h> 


#include <gtsam/slam/dataset .h> 

#include <gtsam/slam/BetweenFactor.h> 

#include <gtsam/slam/PriorFactor.h> 

#include <gtsam/nonlinear/GaussNewton0ptimizer.h> 


#include <gtsam/nonlinear/LevenbergMarquardtOptimizer .h> 


using namespace std; 
using Sophus: :SE3; 
using Sophus: :S03; 


OGIO III III III I TT II AIA IIA. 

* 本 程序 演示 如 何 用 gtsam 进行 位 次 图 优化 

* sphere.g20 是 人 工 生 成 的 一 个 Pose graph， 我 们 来 优化 它 。 
* 与 g20 相似 ， 在 gtsam 中 添加 的 是 因子 ， 相 当 于 误差 。 


* FCG IC IC CRRA AA //- 


int main ( int argc, char** argv ) 


{ 


if ( argc != 2 ) 


{ 
cout<<"Usage: pose_graph_gtsam sphere.g2o"<<end1; 
return 1; 

J 

ifstream fin ( argv[i] ); 

if ( ttin, 

{ 
cout<<"file "<<argv[1]<<" does not exist."<<endl; 
return 1; 

J 


gtsam::NonlinearFactorGraph::shared_ptr graph ( new gtsam::NonlinearFactorGraph 
); // gtsam 的 因子 图 

gtsam::Values::shared_ptr initial ( new gtsam::Values ); // 初始 值 

// 从 9g2o 文 件 中 读 取 节 点 和 边 的 信息 

int cntVertex=0, cntEdge = 0; 


cout<<"reading from g2o file"<<endl; 


while ( !fin.eof() ) 
{ 

string tag; 

fin>>tag; 

if ( tag == "VERTEX_SE3:QUAT" ) 

{ 
// 顶点 
gtsam::Key id; 
fin>>id; 
double data[7] ; 
for ( int i=0; i<7; i++ ) fin>>data[i]; 
// 转换 至 gtsam 的 Pose3 
gtsam::Rot3 R = gtsam::Rot3::Quaternion ( data[6], data[3], ，data[4] ， 
data[5] ); 
gtsam::Point3 t ( data[0], data[i], data[2] ); 
initial->insert ( id, gtsam::Pose3 ( R,t ) ); // 添加 初始 值 
cntVertex++; 

} 

else if ( tag == "EDGE_SE3:QUAT" ) 

{ 
// 边 ， 对 应 到 因子 图 中 的 因子 
gtsam::Matrix m = gtsam::1_6x6; // 信息 和 矩阵 
gtsam: :Key idi, id2; 
fin>>id1>>id2; 
double data[7]; 


for ( int i=0; i<7; i++ ) fin>>data[i]; 


70 gtsam::Rot3 R = gtsam::Rot3::Quaternion ( data[6], data[3], data[4], 


data[5] ); 

7 gtsam::Point3 t ( data[0], data[1], data[2] ); 

72 for ( int i=0; i<6; i++ ) 

73 for ( int j=i; j<6; j++ ) 

74 { 

75 double mij; 

76 fin>>mij; 

77 m (i,j ) = mij; 

78 m ( j,i ) = mij; 

79 } 

80 

81 // g2o 的 信息 和 矩阵 定义 方式 与 gtsam 不 同 ， 这 里 对 它 进 行 修改 

82 gtsam::Matrix mgtsam = gtsam::I_6x6; 

83 mgtsam.block<3,3> ( 0,0 ) = m.block<3,3> ( 3,3 ); // cov rotation 

84 mgtsam.block<3,3> ( 3,3 ) = m.block<3,3> ( 0,0 ); // cov translation 

85 mgtsam.block<3,3> ( 0,3 ) = m.block<3,3> ( 0,3 ); // off diagonal 

86 mgtsam.block<3,3> ( 3,0 ) = m.block<3,3> ( 3,0 ); // off diagonal 

87 

88 // AMR PRA 

89 gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian:: 
Information ( mgtsam ); 

90 gtsam::NonlinearFactor::shared_ptr factor ( 

91 new gtsam::BetweenFactor<gtsam::Pose3> ( idl, id2, gtsam::Pose3 ( R 

t ), model ) // 添加 一 个 因子 

92 di 

93 graph->push_back ( factor ); 

94 cntEdget++; 

95 } 

96 if ( !fin.good() ) 

97 break; 

98 小 

99 

100 cout<<"read total "<<cntVertex<<" vertices, "<<cntEdge<<" edges."<<endl; 

101 // 国定 第 一 个 顶点 ， 在 gtsam 中 相当 于 添加 一 个 先 验 因 子 

102 gtsam::NonlinearFactorGraph graphWithPrior = *graph; 

103 gtsam::noiseModel: :Diagonal::shared_ptr priorModel = 

104 gtsam::noiseModel::Diagonal::Variances ( 

105 ( gtsam::Vector ( 6 ) <<1le-6, le-6, 1e-6, 1e-6, le-6, 1e-6 ).finished() 

106 D's 

107 gtsam::Key firstKey = 0; 

108 for ( const gtsam::Values::ConstKeyValuePair& key_value: *initial ) 

109 { 


110 cout<<"Adding prior to g2o file "<<end1; 
ul graphWithPrior.add ( gtsam::PriorFactor<gtsam::Pose3> ( 


key_value.key, key_value.value.cast<gtsam::Pose3>(), priorModel ) 


a 


break; 


// 开始 因子 图 优化 ， 配 置 优化 选项 

cout<<"optimizing the factor graph"<<end1l; 

// BAVARIA) L1H — BR MR HEM 
gtsam::LevenbergMarquardtParams params_1m; 
params_lm.setVerbosity ("ERROR") ; 
params_lm.setMaxIterations (20) ; 
params_lm.setLinearSolverType("MULTIFRONTAL_QR") ; 


gtsam::LevenbergMarquardtOptimizer optimizer_LM( graphWithPrior, *initial, 


params_lm ); 


AMV 你 可 以 尝试 下 高 斯 牛顿 法 

// gtsam::GaussNewtonParams params_gn; 

// params_gn.setVerbosity ("ERROR"); 

// params_gn.setMarIterations (20) ; 

// params_gn.setLinearSolverType("MULTIFRONTAL_QR") ; 


// gtsam::GaussNewton0ptimizer optimizer ( graphWithPrior, *initial, params_gn 


a; 


gtsam::Values result = optimizer_LM.optimize() ; 
cout<<"Optimization complete"<<end1; 
cout<<"initial error: "<<graph->error ( *initial ) <<endl; 


cout<<"final error: "<<graph->error ( result ) <<endl; 


cout<<"done. write to g2o ... "<<endl; 
// BX g20 文件 ， 同 样 伪装 成 g2o PHM AF, VARI g2o_viewer 查看 
// 顶点 
ofstream fout ( "result_gtsam.g2o" ); 
for ( const gtsam::Values::ConstKeyValuePair& key_value: result ) 
{ 
gtsam::Pose3 pose = key_value.value.cast<gtsam: :Pose3>() ; 
gtsam::Point3 p = pose.translation() ; 
gtsam::Quaternion q = pose.rotation().toQuaternion() ; 
fout<<"VERTEX_SE3:QUAT "<<key_value.key<<" " 
<<p.xQ) <<" Nesp yO <<" “<<p.zq) <<" " 
<<q.x()<<" "<<q.y()<<" "<<q.z()<<" "<<q.wOK<<" "<<endl; 
J 
// ù 
for ( gtsam::NonlinearFactor::shared_ptr factor: *graph ) 


{ 


gtsam: :BetweenFactor<gtsam: :Pose3>::shared_ptr f = dynamic_pointer_cast< 


gtsam: :BetweenFactor<gtsam: :Pose3>>( factor ); 


155 if ( f) 

156 { 

157 gtsam::SharedNoiseModel model = f->noiseModel(); 

158 gtsam::noiseModel::Gaussian::shared_ptr gaussianModel = 


dynamic_pointer_cast<gtsam::noiseModel::Gaussian>( model ); 


159 if ( gaussianModel ) 

160 { 

161 // write the edge information 

162 gtsam::Matrix info = gaussianModel->R().transpose() * gaussianModel 
->R(); 

163 gtsam::Pose3 pose = f->measured() ; 

164 gtsam::Point3 p = pose.translation() ; 

165 gtsam::Quaternion q = pose.rotation().toQuaternion() ; 

166 fout<<"EDGE_SE3:QUAT "<<f->key1()<<" "<<f->key2()<<" " 

167 <<p.x() <<" Wedp yh) A "<<p.zQ) <<" * 

168 <<q.x() <<" "<<q.y()<<" "<<q.z()<<" "<<q.wO <<" "5 

169 gtsam::Matrix infoG2o = gtsam::I_6x6; 

170 infoG2o0.block(0,0,3,3) = info.block(3,3,3,3); // cov translation 

171 infoG2o0.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation 

172 infoG2o.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal 

173 infoG2o.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal 

174 for ( int i=0; i<6; i++ ) 

175 for ( int j=i; j<6; j++ ) 

176 { 

177 fout<<infoG2o(i,j)<<" "; 

178 } 

179 fout<<endl; 

180 } 

181 } 

182 } 

183 fout.close(); 

184 cout<<"done."<<endl; 

185 |} 


在 演示 程序 中 ， 我 们 以 文本 方式 读 取 一 个 g2o 文 件 ， 并 完成 了 向 
gtsam 接 口 的 转换 。 请 留意 代码 中 g2o 与 gtsam 的 异同 点 。 比 如 说 相同 的 地 
方 : 


1. 由 于 它们 本 质 上 都 是 同一 个 最 小 二 乘 优 化 问题 ， 所 以 我 们 要 设置 的 
东西 也 是 类 似 的 。 简 而 言 之 ， 即 顶点 (对 应 到 因子 图 中 的 变量 ) 的 初 
值 ， 以 及 边 (对 应 到 因子 ) 的 观测 值 ， 还 有 噪声 大 小 。 

2. 在 优化 设置 方面 ， 同 样 可 以 使 用 高 斯 牛顿 法 或 列 文 伯 格 一 马 硅 尔 特 
方法 ， 并 配置 详细 的 参数 ， 只 不 过 接口 略 有 不 同 。g2o 通 过 构建 优化 算法 
对 象 来 实现 ， 而 gtsam 则 是 传 入 优化 参数 。 

不 同 的 地 方 : 

1.g2o 的 噪声 模型 和 gtsam 稍 有 不 同 ， 我 们 在 代码 中 做 了 一 下 转换 。 

2. 在 稀 琉 化 的 处 理 方面 ，gtsam 可 选择 使 用 QR 分 解 或 Cholesky 分 解 ， 
尽管 我 们 没有 详细 解释 这 方面 的 具体 过 程 。 读 者 可 以 追踪 参数 的 配置 方 
式 ， 看 看 gtsam 提 供 哪些 求解 方法 。 由 于 单纯 的 Pose Graph 没有 太 多 稀 卜 
性 可 以 利用 ， 所 以 这 里 区 别 不 大 。 

最 后 ， 我 们 把 gtsam 的 优化 结果 转换 为 g2o 的 输出 文件 ， 同 样 可 以 用 
g2o_viewer 查 看 此 结果 ， 如 图 11-9 所 示 。 


Optimize loaded result_gtsam.g2o with 2500 vertices and 9799 measuremen ts 


graph is fixed by node 2499 


图 11-9 gtsam 的 优化 结果 。 
下 面 是 终端 输出 的 优化 信息 : 


三 三 -三 一 三 = 三 -一 
1 |$ build/pose_graph_gtsam Sphere.g2o 


2 |reading from g2o file 

3 |read total 2500 vertices, 9799 edges. 

4 | Adding prior to g2o file 

5 | optimizing the factor graph 

6 | Initial error: 4.7724e+09 

7 |newError: 6.07118e+08 

8 | errorThreshold: 6.07118e+08 > 0 

9 | absoluteDecrease: 4165284178.46 >= 1e-05 
10 | relativeDecrease: 0.872785675288 >= 1e-05 
u | // P% 

12 |newError: 63793.9289983 


13 | errorThreshold: 63793.9289983 > 0 

14 | absoluteDecrease: 0.279685092828 >= 1e-05 

is |relativeDecrease: 4.38417684928e-06 < 1e-05 
16 | converged 

17 | errorThreshold: 63793.9289983 <? 0 

18 | absoluteDecrease: 0.279685092828 <? 1e-05 

19 |relativeDecrease: 4.38417684928e-06 <? 1e-05 
2 |iterations: 5 >? 20 

21 | Optimization complete 

2 |initial error: 4772402087 .24 

23 | final error: 63793.9289982 

24 |done. write to g2o ... 


25 | done. 


我 们 发 现 ， 昌 然 设置 了 最 大 迭代 次 数 为 20 次 ， 但 gtsam 只 迭代 了 5 次 
后 算法 就 收 全 了。 同样 ， 由 于 gtsam 的 误差 定义 方式 与 g20 不 同 ， 所 以 这 
里 直接 比较 误差 大 小 没有 太 大 意义 。 如 果 用 g2o viewer 打开 
result_gtsam.g20 文 件 并 选择 优化 ， 就 会 发 现在 g2o 度 量 下 的 误差 大 约 为 
44360 左 右 ， 和 我 们 上 一 节 使 用 李 代数 优化 的 结果 一 致 ， 但 gtsam 的 迭代 
次 数 更 少 一 些 。 

有 点 遗憾 的 是 ， 本 例 没有 体现 出 gtsam 的 增 量 特性 。 如 果 我 们 把 g20o 
文件 中 的 节点 和 边 按 照 时 间 排 序 ， 然 后 一 个 一 个 地 放 入 gtsam 中 ， 可 能 对 
它 的 增 量 特征 会 有 更 好 的 表述 。 我 们 把 这 个 实验 留 作 习题 ， 交 给 读者 来 


完成 。 


习题 
1. 如 果 将 位 姿 图 中 的 误差 定义 为 ^&; = t,o。&7'， 推 导 按照 此 定义 的 左 乘 
扰动 雅 可 比 和 矩阵 。 


2. 参 照 g20 的 程序 ， 在 Ceres 中 实现 对 “ 球 ” 位 姿 图 的 优化 。 


3. 对 "“ 球 ”中 的 信息 按照 时 间 排 序 ， 分 别 喂 给 g2o0 和 gtsam 优 化 ， 比 较 它 
们 的 性 能 差异 。 


4.* 阅 读 iSAM 相 关 论 文 ， 理 解 它 是 如 何 实现 增 量 式 优化 的 。 
[1] 请 注意 ， 尽 管 数 值 上 看 此 处 的 误差 要 大 一 些 ， 但 是 由 于 我 们 自 定义 边 时 重新 定义 了 误差 的 计算 
方式 ， 所 以 此 处 数值 的 大 小 并 不 能 直接 用 于 比较 。 
[2] 由 于 没有 做 更 多 的 实验 ， 所 以 该 结论 只 在 " 球 " 这 个 例子 上 有 效 。 


第 12 讲 ”回环 检测 


主要 目标 

1. 理 解 回 环 检测 的 必要 性 。 

2. 掌 握 基 于 词 袋 的 外 观 式 回环 检测 。 

3. 通 过 DBoW3 的 实验 ， 学 习 词 袋 模型 的 实际 用 途 。 

本 讲 中 ， 我 们 来 介绍 SLAM 中 另 一 个 主要 模块 : 回环 检测 。 我 们 知道 
SLAM 主 体 (前 端 、 后 端 ) 主要 的 目的 在 于 估计 相机 运动 ， 而 回环 检测 模 
块 ， 无 论 是 目标 上 还 是 方法 上 ， 都 与 前 面 讲 的 内 容 相 差 较 大 ， 所 以 通常 
被 认为 是 一 个 独立 的 模块 。 我 们 将 介绍 主流 视觉 SLAM 中 检测 回环 的 方 
式 : 词 袋 模型 ， 并 通过 DBow 库 上 的 程序 实验 ， 使 读者 得 到 更 加 直观 的 理 
解 。 


12.1 回环 检测 概述 
12.1.1 回环 检测 的 意义 


我 们 已 然 介 绍 了 前 端 和 后 端 : 前 端 提供 特征 点 的 提取 和 轨迹 、 地 图 
的 初 值 ， 而 后 端 负 责 对 所 有 这 些 数 据 进行 优化 。 然 而 ， 如 果 像 VO 那样 仅 
考虑 相 邻 时 间 上 的 关联， 那么 ， 之 前 产生 的 误差 将 不 可 避免 地 累积 到 下 
一 个 时 刻 ， 使 得 整个 SLAM 会 出 现 累积 误差 ， 长 期 估计 的 结果 将 不 可 
T. 或 者 说 ,我们 无 法 构建 全 局 一 致 的 轨迹 和 地 图 。 

举例 来 说 ， 假 设 我 们 在 前 端 提取 了 特征 ， 然 后 忽略 掉 特 征 点 ， 在 后 
端 使 用 PoseGraph 优 化 整个 轨迹 ， 如 图 12-1(a) 所 示 。 由 于 前 端 给 出 的 只 是 
局 部 的 位 姿 间 约束 ， 比如 ， 可 能 是 x， -x , ,x , -Xx ,， 等 等 8 但 是 ， 由 于 x ， 
的 估计 存在 误差 ， 而 x , 是 根据 x ATER, x, 又 是 由 x , 决定 的 。 以 此 类 
推 ， 误 差 融会 被 累积 起 来 ， 使 得 后 端 优 化 的 结果 如 图 12-1(b) 所 示 ， 慢 慢 
地 趋向 不 准确 。 


(a) (b) (c) 


图 12-1 ”漂移 示意 图 。 (a) RR; (b) 由 于 前 端 只 给 出 相 邻 帧 间 的 估计 ， 优 化 后 的 
Pose Graph 出 现 漂 移 ; (c) 添加 回环 检测 后 的 Pose Graph 可 以 消除 累积 误差 。 


里 然后 羡 能 够 估计 最 大 后 验 误差 ， 但 所 谓 “ 好 模型 染 不 住 烂 数据 ”， 
只 有 相 邻 天 键 帧 数据 时 ， 我 们 能 做 的 事情 并 不 很 多 ， 也 无 从 消除 累积 误 
差 。 但 是 ， 回 环 检测 模块 能 够 给 出 除了 相 邻 帧 之 外 的 一 些 时 隔 更 加 久远 
的 约束 : 例如 x; -x io 之 间 的 位 次 变换 。 为 什么 它们 之 间 会 有 约束 呢 ? 这 
是 因为 我 们 察觉 到 相机 经 过 了 同一 个 地 方 ， 采 集 到 了 相似 的 数据 。 而 回 
环 检 测 的 关键 ， 就 是 如 何 有 效 地 检测 出 相机 经 过 同一 个 地 方 这 件 事 。 如 


果 我 们 能 够 成 功 地 检测 到 这 件 事 ， 就 可 以 为 后 端的 Pose Graph 提供 更 多 的 
有 效 数据 ， 使 之 得 到 更 好 的 估计 ， 特 别 是 得 到 一 个 全 局 一 致 (Global 
Consistent) 的 估计 。 由 于 Pose Graph 可 以 看 成 一 个 质点 一 一 弹 笨 系 统 ， 所 
以 回环 检测 相当 于 在 图 像 中 加 入 了 额外 的 弹 赞 ， 提 高 了 系统 稳定 性 。 读 
者 亦 可 直观 地 想象 成 回环 边 把 带 有 累积 误差 的 边 “ 拉 ”到 了 正确 的 位 置 
一 一 如 果 回 环 本 身 正确 的 话 。 


回环 检测 对 于 SLAM 系 统 意义 重大 。 它 关系 到 我 们 估计 的 轨迹 和 地 图 
在 长 时 间 下 的 正确 性 。 另 一 方面 ， 由 于 回环 检测 提供 了 当前 数据 与 所 有 
历史 数据 的 关联 ， 在 跟踪 算法 丢失 之 后 ， 我 们 还 可 以 利用 回环 检测 进行 
重 定 位 。 因 此 ， 回 环 检 测 对 整个 SLAM 系 统 精 度 与 稳健 性 的 提升 是 非常 
明显 的 。 甚 至 在 某 些 时 候 ， 我 们 把 仅 有 前 端 和 局 部 后 端的 系统 称 为 VO， 
而 把 带 有 回环 检测 和 全 局 后 端的 系统 称 为 SLAM。 


12.1.2 方法 


下 面 我 们 来 考虑 回环 检测 如 何 实现 的 问题 。 


最 简单 的 方式 就 是 对 任意 两 幅 图 像 都 做 一 遍 特征 匹配 ， 根 据 正 确 匹 
配 的 数量 确定 哪 两 幅 图 像 存在 关联 一 一 这 确实 是 一 种 朴素 且 有 效 的 思 
想 。 缺 点 在 于 ， 我 们 盲目 地 假设 了 “任意 两 幅 图 像 都 可 能 存在 回环 "， 使 
得 要 检测 的 数量 实在 太 大 : 对 于 NN 个 可 能 的 回环 ， 我 们 要 检测 C?， 那么 


ZR, XEO (N ) 的 复杂 度 ， 随 着 轨迹 变 长 增长 太 快 ， 在 大 多 数 实时 系 
统 当 中 是 不 实用 的 。 另 一 种 朴素 的 方式 是 ， 随 机 抽取 历史 数据 并 进行 回 
环 检测 ， 比 如 说 在 n 帧 当中 随机 抽 5 帧 与 当前 帧 比较 。 这 种 做 法 能 够 维持 
常数 时 间 的 运算 量 ， 但 是 这 种 盲目 试探 方法 在 帧 数 增长 时 ， 抽 到 回环 
的 几率 又 大 幅 下 降 ， 使 得 检测 效率 不 高 。 

上 面 说 的 朴素 思路 都 过 于 粗糙 。 尽 管 随机 检测 在 有 些 实现 中 确实 有 
用 ， 但 我 们 至 少 希望 有 一 个 “ 哪 处 可 能 出 现 回环 ”的 预计 ， 才 好 不 那么 
盲目 地 去 检测 。 这 样 的 方式 大 体 分 为 两 种 思路 : 基于 里 程 计 的 几何 关系 

(Odometry based) ， 或 基于 外 观 (Appearance based) 。 基 于 几何 关系 是 
说 ， 当 我 们 发 现 当 前 相机 运动 到 了 之 前 的 某 个 位 置 附 近 时 ， 检 测 它们 有 
没有 回环 关系 9 一 一 这 自然 是 一 种 直观 的 想法 ， 但 是 由 于 累积 误差 的 存 
在 ， 我 们 往往 没 法 正确 地 发 现 “ 运 动 到 了 之 前 的 某 个 位 置 附 近 ” 这 件 事 
实 ， 回 环 检测 也 无 从 谈 起 。 因 此 ， 这 种 做 法 在 逻辑 上 存在 一 点 问题 ， 


为 回环 检测 的 目标 在 于 发 现 “相机 回 到 之 前 位 置 * 的 事实 ， 从 而 消除 累积 
误差 。 而 基于 几何 关系 的 做 法 假设 了 “相机 回 到 之 前 位 置 附近 *， 这 样 才 
能 检测 回环 。 这 是 有 倒 果 为 因 的 嫌疑 的 ， 因 而 也 无 法 在 累积 误差 较 大 时 
THe 


另 一 种 方法 是 基于 外 观 的 。 它 和 前 端 、 后 端的 估计 都 无 关 ， 仅 根据 
两 幅 图 像 的 相似 性 确定 回环 检测 关系 。 这 种 做 法 摆脱 了 累积 误差 ， 使 回 
环 检测 模块 成 为 SLAM 系 统 中 一 个 相对 独立 的 模块 (当然 前 端 可 以 为 它 提 
供 特征 点 ) 。 自 21 世 纪 初 被 提出 以 来 ， 基 于 外 观 的 回环 检测 方式 能 够 有 
效 地 在 不 同 场 景 下 工作 ， 成 为 了 视觉 SLAM 中 主流 的 做 法 ， 并 被 应 用 于 实 
际 的 系统 中 去 93%73] 。 


在 基于 外 观 的 回环 检测 算法 中 ， 核 心 问题 是 如 何 计算 图 像 间 的 相似 
性 。 比 如 ， 对 于 图 像 A 和 图 像 B ， 我 们 要 设计 一 种 方法 ， 计 算 它们 之 间 
的 相似 性 评分 : s (4,B )。 当 然 这 个 评分 会 在 某 个 区 间 内 取 值 ， 当 它 大 于 
一 定量 后 我 们 认为 出 现 了 一 个 回环 。 读 者 可 能 会 有 疑问 : 计算 两 幅 图 像 
之 间 的 相似 性 很 困难 吗 ?” 例 如 直观 上 看 ， 图 像 能 够 表示 成 忠 阵 ， 那 么 直 
接 让 两 幅 图 像 相 减 ， 然 后 取 某 种 范 数 行 不 行 呢 ? 


s(A, B) = ||A— BI). (12.1) 


为 什么 我 们 不 这 样 做 ? 


1. 首 先 ， 前 面 也 说 过 ， 像 素 灰 度 是 一 种 不 稳定 的 测量 值 ， 它 严重 地 受 
环境 光照 和 相机 曝光 的 影响 。 假 设 相机 未 动 ， 我 们 打开 了 一 广电 灯 ， 那 
么 图 像 会 整体 变 亮 一 些 。 这 样 ， 即 使 对 于 同样 的 数据 ， 我 们 都 会 得 到 一 
个 很 大 的 差异 值 。 


2. 另 一 方面 ， 当 相机 视角 发 生 少 量变 化 时 ， 即 使 每 个 物体 的 光度 不 
变 ， 它 们 的 像素 也 会 在 图 像 中 发 生 位 移 ， 造 成 一 个 很 大 的 差异 值 。 


由 于 这 两 种 情况 的 存在 ， 实 际 当 中 ， 即 使 对 于 非常 相似 的 图 像 ，A-B 
也 会 经 常 得 到 一 个 (不 符合 实际 的 ) 很 大 的 值 。 所 以 我 们 说 ， 这 个 函数 
不 能 很 好 地 反映 图 像 间 的 相似 关系 。 这 里 牵涉 到 一 个 “好 * 和 “不 好 ”的 定 
义 问 题 。 我 们 要 问 ， 怎 样 的 图 数 能 够 更 好 地 反映 相似 关系 ， 而 怎样 的 冰 
数 不 够 好 呢 ? 从 这 里 可 以 引出 感知 偏差 (Perceptual Aliasing) 和 感知 变 
异 (PerceptualVariability) 两 个 概念 。 现 在 我 们 来 更 详细 地 讨论 一 下 。 


12.1.3 ”准确 率 和 召回 率 


从 人 类 的 角度 看 ， (至 少 我 们 自 认为 ) 我 们 能 够 以 很 高 的 精确 度 ， 
感觉 到 “两 幅 图 像 是 否 相 似 ” 或 “这 两 张 照 片 是 从 同一 个 地 方 拍摄 的 ”这 一 事 
实 ， 但 由 于 目前 尚未 掌握 人 脑 的 工作 原理 ， 我 们 无 法 清楚 地 描述 自己 是 
如 何 完 成 这 件 事 的 。 从 程序 角度 看 ， 我 们 希望 程序 算法 能 够 得 出 和 人 
类 ， 或 者 和 事实 一 致 的 判断 。 当 我 们 觉得 ， 或 者 事实 上 就 是 ， 两 幅 图 像 
从 同一 个 地 方 拍摄 ， 那 么 回环 检测 算法 也 应 该 给 出 “这 是 回环 ”的 结果 。 
有 反之， 如 果 我 们 觉得 ， 或 事实 上 是 ， 两 幅 图 像 是 从 不 同 地 方 拍摄 的 ， 那 
么 程序 也 应 该 给 出 “这 不 是 回环 ”的 判断 。 中 当然， 程序 的 判断 并 不 总 是 
我 们 人 类 的 想法 一 致 ， 所 以 可 能 出 现 表 12-1 中 的 4 种 情况 : 


表 12-1 回 环 检 测 的 结果 分 类 


算法 \ 事实 不 是 回环 
是 回环 真 阳性 (True Positive ) | 假 阳 性 (False Positive ) 
不 是 回环 | 假 阴 性 ( False Negative ) | 真 阴性 (True Negative ) 


这 里 阴性 /阳性 的 说 法 是 借用 了 医学 上 的 说 法 。 假 阳性 (False 
Positive) 又 称 为 感知 偏差 ， 而 假 阴 性 (False Negative) 称 为 感知 变异 
( 见 图 12-2) 。 为 方便 书写 ， 用 缩写 TP 代 表 True Positive， 其 余 类 推 。 由 
于 我 们 希望 算法 和 人 类 的 判断 一 致 ， 所 以 希望 TP 和 TN 要 尽量 高 ， 而 FP 和 
FN 要 尽 可 能 低 。 所 以 ， 对 于 某 种 特定 算法 ， 我 们 可 以 统计 它 在 某 个 数据 
集 上 的 TP、TN、FP、FN 的 出 现 次 数 ， 并 计算 两 个 统计 量 : 准确 率 和 召 
回 率 (Precision&Recall) o 


Precision = TP/(TP+ FP), Recall = TP/(TP + EN). (12.2) 


False Positive False Negative 
12-2 ” 假 阳性 与 假 阴 性 的 例子 。 左 侧 为 假 阳性 ， 两 幅 图 像 看 起 来 很 像 ， 但 并 非 同 一 走廊 ; 
右 侧 为 假 阴 性 ， 由 于 光照 变化 ， 同 一 地 点 不 同时 刻 的 照片 看 起 来 很 不 一 样 。 


从 公式 字面 意义 上 来 看 ， 准 确 率 描 述 的 是 ， 算 法 提取 的 所 有 回环 中 
确实 是 真实 回环 的 概率 。 而 召回 率 则 是 说 ， 在 所 有 真实 回环 中 被 正确 检 
测 出 来 的 概率 。 为 什么 取 这 两 个 统计 量 呢 ? 因为 它们 有 一 定 的 代表 性 ， 
并 且 通 常 来 说 是 一 对 矛盾 。 

一 个 算法 往往 有 许多 的 设置 参数 。 比 如 说 ， 当 提高 某 个 靖 值 时 ， 算 
法 可 能 变 得 更 加 “严格 ”一 一 它 检 出 更 少 的 回环 ， 使 准确 率 得 以 提高 。 但 
同时 ， 由 于 检 出 的 数量 变 少 了 ， 许 多 原本 是 回环 的 地 方 就 可 能 被 漏 掉 
了 ， 导 致 召回 率 下 降 。 反 之 ， 如 果 我 们 选择 更 加 宽松 的 配置 ， 那 么 检 出 
的 回环 数量 将 增加 ， 得 到 更 高 的 召回 率 ， 但 其 中 可 能 混杂 一 些 不 是 回环 
的 情况 ， 于 是 准确 率 下 降 。 


为 了 评价 算法 的 好 坏 ， 我 们 会 测试 它 在 各 种 配置 下 的 P 和 R 值 ， 然 后 
做 出 一 条 Precision-Recall 曲 线 ( 见 图 12-3) 。 当 用 召回 率 为 横 轴 ， 用 准确 
率 为 纵 轴 时 ， 我 们 会 关心 整 条 曲线 偏向 右上 方 的 程度 、1009% 准 确 率 下 的 
召回 率 或 者 50% 召 回 率 时 的 准确 率 ， 作 为 评价 算法 的 指标 。 不 过 请 注意 ， 
除去 一 些 “ 天 壤 之 别 * 的 算法 ， 我 们 通常 不 能 一 概 而 论 地 说 算法 A 就 是 优 于 
算法 B 的 。 我 们 可 能 会 说 A 在 准确 率 较 高 时 还 有 很 好 的 召回 率 ， 而 B 在 
70% 召 回 率 的 情况 下 还 能 保证 较 好 的 准确 率 ， 诸 如 此 类 。 


值得 一 提 的 是 ， 在 SLAM 中 ， 我 们 对 准确 率 要 求 更 高 ， 而 对 召回 率 则 
相对 宽容 一 些 。 由 于 假 阳 性 的 (检测 结果 是 而 实际 不 是 的 ) 回环 将 在 后 
端的 Pose Graph 中 添加 根本 错误 的 边 ， 有 些 时 候 会 导致 优化 算法 给 出 完全 
错误 的 结果 。 想 象 一 下 ， 如 果 SLAM 程 序 错误 地 将 所 有 的 办 公 桌 当成 了 同 
一 张 ， 那 建 出 来 的 图 会 怎么 样 呢 ? 你 可 能 会 看 到 走廊 不 直 了 ， 墙 壁 被 交 
错 在 一 起 了 ， 最 后 整个 地 图 都 失效 了 。 而 相 比 之 下 ， 召 回 率 低 一 些 ， 则 
顶 多 有 部 分 的 回环 没有 被 检测 到 ， 地 图 可 能 受 一 些 累 积 误 差 的 影响 
然而 仅 需 一 两 次 回环 就 可 以 完全 消除 它们 了 。 所 以 说 在 选择 回环 检测 算 
法 时 ， 我 们 更 倾向 于 把 参数 设置 得 更 严格 一 些 ， 或 者 在 检测 之 后 再 加 上 
回环 验证 的 步骤 。 
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图 12-3 ”准确 率 -召回 率 曲线 的 例子 e 。 随 着 召回 率 的 上 升 ， 检 测 条 件 变 得 宽松 ， 准 确 率 随 
之 下 降 。 好 的 算法 在 较 高 召回 率 情况 下 仍 能 保证 较 好 的 准确 率 。 


那么 ， 回 到 之 前 的 问题 ， 为 什么 不 用 A-B 来 计算 相似 性 呢 ? 我 们 会 发 
现 它 的 准确 率 和 召回 率 都 很 差 ， 可 能 出 现 大 量 的 False Positive 或 False 
Negative 的 情况 ， 所 以 说 这 样 做 “不 好 ”。 那 么 ， 什 么 方法 更 好 一 些 呢 ? 


12.2” 词 袋 模 型 


既然 直接 用 两 张 图 像 相 减 的 方式 不 够 好 ， 那 么 我 们 就 需要 一 种 更 加 
可 靠 的 方式 。 结 合 前 面 几 讲 的 内 容 ， 一 种 直观 的 思路 是 : 为 何不 像 VO 那 
样 使 用 特征 点 来 做 回环 检测 呢 ? 和 VO 一 样 ， 我 们 对 两 幅 图 像 的 特征 点 进 
行 匹 配 ， 只 要 匹配 数量 大 于 一 定 值 ， 就 认为 出 现 了 回环 。 进 一 步 ， 根 据 
特征 点 匹配 ， 我 们 还 能 计算 出 这 两 幅 图 像 之 间 的 运动 关系 。 当 然 这 种 做 
法 会 存在 一 些 问题 ， 例 如 ， 特 征 的 匹配 会 比较 费时 、 当 光照 变化 时 特征 
描述 可 能 不 稳定 等 ， 但 离 我 们 要 介绍 的 词 袋 模型 已 经 很 相近 了 。 下 面 我 
们 先 来 讲 词 袋 的 做 法 ， 表 来 讨论 数据 结构 之 类 的 实现 细节 。 

词 袋 ， 也 就 是 Bag-of-Words (BoW) ， 目 的 是 用 “图 像 上 有 哪 几 种 特 
征 ” 来 描述 一 幅 图 像 。 例 如 ， 我 们 说 某 张 照片 中 有 一 个 人 、 一 辆 车 ; 而 另 
一 张 中 有 两 个 人 、 一 只 狗 。 根 据 这 样 的 描述 ， 就 可 以 度量 这 两 幅 图 像 的 
相似 性 。 再 具体 一 些 ， 我 们 要 做 以 下 几 件 事 : 


1. 确 定 “< 人 ”和 车 ”“ 狗 ”等 概念 一 一 对 应 于 BoW 中 的 “单词 ”(Word) ， 许 
多 单词 放 在 一 起 ， 组 成 了 “字典 ”(Dictionary) o 


2. 确 定 一 幅 图 像 中 出 现 了 哪些 在 字典 中 定义 的 概念 一 一 我 们 用 单词 出 
现 的 情况 (或 直方 图 ) 描述 整 幅 图 像 。 这 就 把 一 幅 图 像 转换 成 了 一 个 向 
量 的 描述 。 

3. 比 较 上 一 步 中 的 描述 的 相似 程度 。 

以 上 面 举 的 例子 来 说 ， 首 先 我 们 通过 某 种 方式 得 到 了 一 本 “字典 ”。 
字典 上 记录 了 许多 单词 ， 每 个 单词 都 有 一 定 意义 ， 例 如 “人 ”车 “和 狗 ” 都 是 
记录 在 字典 中 的 单词 ， 我 们 不 妨 记 为 w , w, ,w ;。 然 后 ， 对 于 任意 图 像 
A， 根 据 它们 含有 的 单词 ， 可 记 为 


字典 是 固定 的 ， 所 以 只 要 用 [1, 1, 0]’ 这 个 向 量 就 可 以 表达 A 的 意义 。 
通过 字典 和 单词 ， 只 需 一 个 向 量 就 可 以 描述 整 幅 图 像 了 。 该 向 量 描述 的 
是 “图 像 是 否 含有 某 类 特征 ”的 信息 ， 比 单纯 的 灰 度 值 更 加 稳定 。 又 因为 
描述 向 量 说 的 是 “是 否 出 现 ”， 而 不 管 它们 “在 哪儿 出 现 ”， 所 以 与 物体 的 
空间 位 置 和 排列 顺序 无 天 ， 因 此 在 相机 发 生 少 量 运 动 时 ， 只 要 物体 仍 在 
视野 中 出 现 ， 我 们 就 仍然 保证 描述 向 量 不 发 生变 化 。 包 基于 这 种 特性 ， 
我 们 称 它 为 Bag-of-Words 而 不 是 什么 List-of-Words ， 强 调 的 是 Words 的 有 
无 ， 而 无 天 其 顺序 。 因 此 ， 可 以 说 字典 类 似 于 单词 的 一 个 集合 。 

回 到 上 面 的 例子 ， 同 理 ， 用 [2, 0, 1 可 以 描述 图 像 B 。 如 果 只 考虑 
“是 否 出 现 ” 而 不 考虑 数量 ， 也 可 以 是 [1 0, 1]7 ， 这 时 候 这 个 向 量 就 是 二 值 
的 。 于 是 ， 根 据 这 两 个 向 量 ， 设 计 一 定 的 计算 方式 ， 就 能 确定 图 像 间 的 
相似 性 了 。 当 然 ， 如 果 对 两 个 向 量 求 差 仍然 有 一 些 不 同 的 做 法 ， 比 如 对 
于 abpERY ， 可 以 计算 : 


1 
s(a,b)=1— w le~ ll - (12.4) 


其 中 范 数 取 L , 范 数 ， 即 各 元 素 绝对 值 之 和 。 请 注意 在 两 个 向 量 完 
一 样 时 ， 我 们 将 得 到 1; 完全 相反 时 (a 为 0 的 地 方 b 为 1) 得 到 0。 这 样 就 
定义 了 两 个 描述 向 量 的 相似 性 ， 也 就 定义 了 图 像 之 间 的 相似 程度 。 

接 下 来 的 问题 是 什么 呢 ? 


1. 我 们 虽然 清楚 了 字典 的 定义 方式 ， 但 它 到 底 是 怎么 来 的 呢 ? 

2. 如 果 我 们 能 够 计算 两 幅 图 像 间 的 相似 程度 评分 ， 是 否 就 足够 判断 回 
KT WE? 

所 以 接 下 来 ， 我 们 首先 介绍 字典 的 生成 方式 ， 然 后 介绍 如 何 利用 字 
典 实 际 地 计算 两 幅 图 像 间 的 相似 性 。 


12.3 ”字典 


12.3.1 ”字典 的 结构 


按照 前 面 的 介绍 ， 字 典 由 很 多 单词 组 成 ， 而 每 一 个 单词 代表 了 一 个 
概念 。 一 个 单词 与 一 个 单独 的 特征 点 不 同 ， 它 不 是 从 单 幅 图 像 上 提取 出 
来 的 ， 而 是 某 一 类 特征 的 组 合 。 所 以 ， 字 上 典 生 成 问题 类 似 于 一 个 聚 类 

(Clustering) 问题 。 

聚 类 问题 在 无 监督 机 器 学 习 (Unsupervised ML) 中 特别 常见 ， 用 于 
让 机 器 自行 寻找 数据 中 的 规律 。BoW 的 字典 生成 问题 亦 属 于 其 中 之 一 。 
首先 ， 假 设 我 们 对 大 量 的 图 像 提 取 了 特征 点 ， 比 如 说 有 NN 个。 现在， 我 
们 想 找 一 个 有 K 个 单词 的 字典 ， 每 个 单词 可 以 看 作 局 部 相 邻 特征 点 的 集 
合 ， 应 该 怎么 做 呢 ? 这 可 以 用 经 典 的 K-means 〈K 均 值 ) 算法 吧 解决 。 

K-means 是 一 个 非常 简单 有 效 的 方法 ， 因 此 在 无 监督 学 习 中 广 为 使 
用 ,下面 对 其 原理 稍 做 介绍 。 简 单 的 说 ， 当 有 NN 个 数据 ， 想 要 归 成 k 个 
类 ， 那 么 用 K-means 来 做 主要 包括 如 下 步骤 : 

1. 随 机 选取 k 个 中 心 点 : c,,…,c,。 

2. 对 每 一 个 样本 ， 计 算 它 与 每 个 中 心 点 之 间 的 距离 ， 取 最 小 的 作为 它 
的 归 类 。 

3. 重 新 计算 每 个 类 的 中 心 点 。 

4. 如 果 每 个 中 心 点 都 变化 很 小 ， 则 算法 收效 ， 退 出 ; 否则 返回 第 2 
步 。 

K-means 的 做 法 是 朴素 且 简 单 有 效 的 ， 不 过 也 存在 一 些 问 题 ， 例 如 ， 
需要 指定 聚 类 数量 、 随 机 选取 中 心 点 使 得 每 次 聚 类 结果 都 不 相同 ， 以 及 


一 些 效率 上 的 问题 。 随 后 研究 者 们 亦 开 发 出 了 层次 聚 类 法 、 开 -means++[e3l 
等 算法 以 弥补 它 的 不 足 ， 不 过 这 都 是 后 话 ， 我 们 就 不 详细 讨论 了 。 总 
之 ， 根 据 K-means ， 我 们 可 以 把 已 经 提取 的 大 量 特征 点 聚 类 成 一 个 含有 k 
个 单词 的 字典 了 。 现 在 的 问题 变 成 了 如 何 根据 图 像 中 某 个 特征 点 查找 字 
典 中 相应 的 单词 。 

仍然 有 朴素 的 思想 : 只 要 和 每 个 单词 进行 比 对 ， 取 最 相似 的 那个 就 
可 以 了 一 一 这 当然 是 简单 有 效 的 做 法 。 然 而 ， 考 虑 到 字典 的 通用 性 外 ， 
我 们 通常 会 使 用 一 个 较 大 规模 的 字典 ， 以 保证 当前 使 用 环境 中 的 图 像 特 
征 都 曾 在 字典 里 出 现 ， 或 至 少 有 相近 的 表达 。 如 果 你 觉得 对 十 个 单词 一 
一 比较 不 是 什么 麻烦 事 ， 那 么 对 于 一 万 个 呢 ? 十 万 个 呢 ? 

也 许 读 者 学 过 数据 结构 ， 这 种 O (n ) 的 查找 算法 显然 不 是 我 们 想 要 
的 。 如 果 字 典 排 过 序 ， 那 么 二 分 查找 显然 可 以 提升 查找 效率 ， 达 到 对 数 
级 别 的 复杂 度 。 而 实践 当中 ， 我 们 可 能 会 用 更 复杂 的 数据 结构 ， 例 如 
Fabmap!+981 中 的 Chou-Liu tree”! 等 。 但 我 们 不 想 把 本 书写 成 复杂 细节 的 
集合 ， 所 以 介绍 另 一 种 较为 简单 实用 的 树 结构 B93 。 

在 文献 [98] 中 ， 使 用 一 种 k 叉 树 来 表达 字典 。 它 的 思路 很 简单 (如 图 
12-4 所 示 ) ， 类 似 于 层次 聚 类 ， 是 k-means 的 直接 扩展 。 假 定 我 们 有 NN 个 
特征 点 ， 希 望 构 建 一 个 深度 为 d 、 每 次 分 叉 为 k 的 树 ， 那 么 做 法 如 下 中 : 

1. 在 根 节点 ， 用 k-means 把 所 有 样本 聚 成 k 类 (实际 中 为 保证 聚 类 均 义 
性 会 使 用 k-means++) 。 这 样 得 到 了 第 一 层 。 

2. 对 第 一 层 的 每 个 节点 ， 把 属于 该 节点 的 样本 再 聚 成 k 类 ， 得 到 下 一 


3. 以 此 类 推 ,， 最 后 得 到 叶子 层 。 叶 子 层 即 为 所 谓 的 Words。 


根 节 点 


查找 某 特征 时 的 路 径 


图 12-4 K 叉 树 字 典 示意 图 。 训 练 字典 时 ， 逐 层 使 用 K-means 聚 类 。 根 据 已 知 特征 查找 单词 
时 ， 亦 可 逐 层 比 对 ， 找 到 对 应 的 单词 。 


实际 上 ， 最 终 我 们 仍 在 叶子 层 构建 了 单词 ， 而 树 结构 中 的 中 间 市 点 
仅 供 快速 查找 时 使 用 。 这 样 一 个 k 分 支 、 深 度 为 d 的 树 ， 可 以 容纳 ki 个 单 
词 。 另 一 方面 ， 在 查找 某 个 给 定 特征 对 应 的 单词 时 ， 只 需 将 它 与 每 个 中 
间 节 点 的 聚 类 中 心 比 较 (一 共 q 次 ) ， 即 可 找到 最 后 的 单词 ， 保 证 了 对 数 
级 别 的 查找 效率 。 


12.3.2 ”实践 : 创建 字典 


既然 讲 到 了 字典 生成 ， 我 们 就 来 实际 演示 一 下 。 前 面 的 VO 部 分 大 量 
使 用 了 ORB 特 征 描 述 ， 所 以 这 里 就 来 壮 示 一 下 如 何 生 成 及 使 用 ORB 字 
HA 


一 、O 


本 实验 中 ， 我 们 选取 TUM 数 据 集 中 的 10 幅 图 像 (位 于 
slambook/ch9/data 中 ， 如 图 12-5 所 示 ) ， 它 们 来 自 一 组 实际 的 相机 运动 轨 
迹 。 可 以 看 出 ， 第 一 幅 图 像 与 最 后 一 幅 图 像 明 显 采 自 同一 个 地 方 ， 现 在 
我 们 要 看 程序 能 否 检 测 到 这 件 事情 。 根 据 词 袋 模 型 ， 我 们 先 来 生成 这 十 
张 图 像 对 应 的 字典 。 
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图 12-5 ”演示 实验 中 使 用 的 十 幅 图 像 ， 采 集 自 不 同时 刻 的 轨迹 。 


需要 声明 的 是 ， 实 际 BoW 使 用 时 字典 往往 是 从 更 大 的 数据 集 生成 
的 ， 而 且 最 好 是 来 自 与 目标 环境 类 似 的 地 方 。 我 们 通 单 使 用 较 大 规模 的 
字典 一 一 越 大 代表 字典 单词 量 越 丰 富 ， 越 容易 找到 与 当前 图 像 对 应 的 单 
词 ， 但 也 不 能 大 到 超过 我 们 的 计算 能 力 和 内 存 。 笔 者 不 打算 在 GitHub 上 
存放 一 个 很 大 的 字典 文件 ， 所 以 我 们 暂时 从 十 幅 图 像 训练 一 个 小 的 字 
典 。 如 果 读 者 想 进 一 步 追 求 更 好 的 效果 ， 应 该 下 载 更 多 的 数据 ， 训 练 更 
大 的 字典 ， 这 样 程序 才 会 实用 。 也 可 以 使 用 别人 训练 好 的 字典 ， 但 请 注 
意 字 典 使 用 的 特征 类 型 是 否 一 致 。 


下 面 开 始 训练 字 典 。 首 先 ， 请 安装 本 程序 使 用 的 BoWw 库 。 我 们 使 用 
AY DBoW3"! : https:/github.com/rmsalinas 人 DBow3。 读者 也 可 从 本 书 代 
码 的 3rdparty 文 件 夹 中 找到 它 ， 它 是 一 个 cmake 工 程 。 


接 下 来 考虑 训练 字典 : 
由 slambook/ch9/feature_training.cpp 


int main( int argc, char** argv ) 
{ 
// read the image 
cout<<"reading images... "<<endl; 
vector<Mat> images; 
for ( int i=0; i<10; i++ ) 


{ 


N a wu > w N 一 


8 string path = "./data/"+to_string(i+1)+".png"; 
9 images.push_back( imread(path) ) ; 

10 } 

11 

12 // detect ORB features 

13 cout<<"detecting ORB features ... "<<endl; 
14 Ptr< Feature2D > detector = ORB::create(); 
15 vector<Mat> descriptors; 

16 for ( Mat& image:images ) 

17 { 

18 vector<KeyPoint> keypoints; 

19 Mat descriptor; 

20 detector->detectAndCompute( image, Mat(), keypoints, descriptor ); 
21 descriptors.push_back( descriptor ); 
22 } 

23 

24 // create vocabulary 

25 cout<<"creating vocabulary ... "<<endl; 

26 DBoW3::Vocabulary vocab; 

27 vocab.create( descriptors ); 

28 cout<<"vocabulary info: "<<vocab<<endl; 

29 vocab.save( "vocabulary.yml.gz" ); 

30 cout<<"done"<<end1; 

31 

32 return 0; 

3 |} 


DBoW3 的 使 用 非常 容易 。 我 们 对 十 张 目标 图 像 提 取 ORB 特 征 并 存放 
至 vector 容器 中 ， 然 后 调用 DBoW3 的 字典 生成 接口 即 可 。 在 
DBoW3::Vocabulary 对 象 的 构造 水 数 中 ， 我 们 能 够 指定 树 的 分 又 数量 及 
度 ， 不 过 这 里 使 用 了 默认 构造 冰 数 ， 也 就 是 k =10,d =5。 这 是 一 个 小 规模 
的 字典 ， 最 大 能 容纳 10,000 个 单词 。 对 于 图 像 特 征 ， 我 们 亦 使 用 默认 参 
数 ， 即 每 幅 图 像 500 个 特征 点 。 最 后 ， 我 们 把 字典 存储 为 一 个 压缩 文件 。 


运行 此 程序 ， 将 看 到 如 下 字典 信息 输出 : 


1 |$ build/feature_training 

2 |reading images... 

3 |detecting ORB features ... 

4 |creating vocabulary ... 

5 | vocabulary info: Vocabulary: k = 10, L = 5, Weighting = tf-idf, Scoring = Li-norm, 
Number of words = 4983 


6 | done 


我 们 看 到 : 分 支 数 量 k 为 10， 深 度 L AS! ， 单 词 数量 为 4983， 没 有 
达到 最 大 容量 。 但 是 ， 剩 下 的 Weighting 和 Scoring 是 什么 呢 ? 从 字面 上 
看 ，Weighting 是 权重 ，Scoring 似 乎 指 的 是 评分 ， 但 评分 是 如 何 计 算 的 
呢 ? 


12.4 ”相似 度 计 算 
12.4.1 理论 部 分 


下 面 我 们 来 讨论 相似 度 计 算 的 问题 。 有 了 字典 之 后 ， 给 定 任意 特征 ff 
， 只 要 在 字典 树 中 逐 层 查找 ， 最 后 都 能 找到 与 之 对 应 的 单词 w, 一 一 当 字 
典 足够 大 时 ， 我 们 可 以 认为 f Aw, 来 自 同一 类 物体 (尽管 没有 理论 上 的 保 
证 ， 仅 是 在 聚 类 意义 下 这 样 说 ) 。 那 么 ， 假 设 一 幅 图 像 中 提取 了 N 个 特 
征 ， 找 到 这 NN 个 特征 对 应 的 单词 之 后 ， 我 们 就 相当 于 拥有 了 该 图 像 在 单 
词 列表 中 的 分 布 ， 或 者 直方 图 。 直 观 地 讲 (或 理想 情况 下 ) ， 相 当 于 是 
说 “这 幅 图 里 有 一 个 人 和 一 辆 汽车 ”这 样 的 意思 了 。 根 据 Bag-of-Words 的 说 
法 ， 不 妨 认为 这 是 一 个 Bag。 


注意 到 这 种 做 法 中 我 们 对 所 有 单词 都 是 “一 视 同仁 ”的 一 一 有 就 是 
有 ， 没 有 就 是 没有 。 这 样 做 好 不 好 呢 ? 考虑 到 不 同 的 单词 在 区 分 性 上 的 
重要 性 并 不 相同 。 例 如 ,， “的 “是 ”这 样 的 字 可 能 在 许 许多 多 的 句子 中 出 
现 ， 我 们 无 法 根据 它们 判别 句子 的 类 型 ， 但 如 果 有 “文档 “足球 ”这 样 的 单 
词 ， 对 判别 句子 的 作用 就 大 一 些 ， 可 以 说 它们 提供 了 更 多 信息 。 所 以 概 
括 起 来 ， 我 们 希望 对 单词 的 区 分 性 或 重要 性 加 以 评估 ， 给 它们 不 同 的 权 
值 以 起 到 更 好 的 效果 。 


在 文本 检索 中 ， 常 用 的 一 种 做 法 称 为 TF-IDF (Term Frequency- 
Inverse Document Frequency) "00 ， 或 译 频 率 - 道 文档 频率 中 。TF 部 分 的 
思想 是 ， 某 单词 在 一 幅 图 像 中 经 常 出 现 ， 它 的 区 分 度 就 高 。 另 一 方面 ， 
IDF 的 思想 是 ， 某 单词 在 字典 中 出 现 的 频率 越 低 ， 则 分 类 图 像 时 区 分 度 越 
[Slo 

在 词 袋 模型 中 ， 在 建立 字典 时 可 以 考虑 IDF 部 分 。 我 们 统计 某 个 叶子 
节点 w, 中 的 特征 数量 相对 于 所 有 特征 数量 的 比例 作为 IDF 部 分 。 假 设 所 有 
特征 数量 为 n ，w, 数量 为 mw ， 那 么 该 单词 的 IDF 为 


IDF; = log —. (12.5) 
Ni 


另 一 方面 ，TF 部 分 则 是 指 某 个 特征 在 单 幅 图 像 中 出 现 的 频率 。 假 设 
图 像 A 中 单词 w 出 现 了 m 次 ， 而 一 共 出 现 的 单词 次 数 为 n ， 那 么 TF 为 


maen (12.6) 
n 


于 是 ，w, 的 权重 等 于 TF 乘 IDF 之 积 : 
ni = TF; x IDE;. (12.7) 


考虑 权重 以 后 ， 对 于 某 幅 图 像 A ， 它 的 特征 点 可 对 应 到 许多 个 单词 ， 
组 成 它 的 Bagof-Words: 


A = {(w1, q), (we, 72),---, (wn, NN)} > (12.8) 


由 于 相似 的 特征 可 能 落 到 同一 个 类 中 ， 因 此 实际 的 w 中 会 存在 大 量 
的 零 。 无 论 如 何 ， 通 过 词 袋 我 们 用 单个 向 量 w 描述 了 一 幅 图 像 A。 这 个 
HS ENARE, CHIENET SARA 中 含有 哪些 单 
词 ， 而 这 些 部 分 的 值 为 TF-IDF 的 值 。 

接 下 来 的 问题 是 : Ev, 和 vs ， 如 何 计算 它们 的 差异 呢 ? 这 个 问题 
和 沁 数 定义 的 方式 一 样 ， 存 在 若干 种 解决 方式 ， 比 如 文献 [102] 中 提 到 的 L 
EAT: 


s(v4a — vp) =2 > [vail + |vBi| — |Vai — vBil. (12.9) 
4ADARSMANANSMRA, ERERMNMA-GEA 
示 。 至 此 ， 我 们 已 说 明了 如 何 通 过 词 袋 模型 来 计算 任意 图 像 间 的 相似 
度 。 下 面 通 过 程序 实际 演练 一 下 。 


12.4.2 ”实践 : 相似 度 的 计算 


上 一 节 的 实践 部 分 中 ， 我 们 已 对 十 幅 图 像 生 成 了 字典 。 这 次 我 们 使 
用 此 字典 生成 Bag-of-Words 并 比较 它们 的 差异 ， 看 看 与 实际 有 什么 不 同 。 


内 slambook/ch9/loop_closure.cpp 


1 | int main( int argc, char** argv ) 

2 |{ 

3 // read the images and database 

4 cout<<"reading database"<<end1; 

5 DBoW3: : Vocabulary vocab("./vocabulary.yml.gz") ; 
6 if ( vocab.empty() ) 

7 { 

8 cerr<<"Vocabulary does not exist."<<endl; 

9 return 1; 

10 } 

11 cout<<"reading images... "<<endl; 

12 vector<Mat> images; 

13 for ( int i=0; i<10; i++ ) 

14 { 

15 string path = "./data/"+to_string(it+1)+".png"; 


images.push_back( imread(path) ) ; 


// NOTE: in this case we are comparing images with a vocabulary generated by 


themselves, this may leed to overfitting. 
// detect ORB features 

cout<<"detecting ORB features ... "<<endl; 
Ptr< Feature2D > detector = ORB::create(); 
vector<Mat> descriptors; 


for ( Mat& image:images ) 


{ 
vector<KeyPoint> keypoints; 
Mat descriptor; 
detector->detectAndCompute( image, Mat(), keypoints, descriptor ); 
descriptors .push_back( descriptor ); 
} 


// we can compare the images directly or we can compare one image to a database 


// images 
cout<<"comparing images with images "<<endl; 


for ( int i=0; i<images.size(); i++ ) 


{ 
DBoW3::BowVector vi; 
vocab.transform( descriptors[i], vi ); 
for ( int j=i; j<images.size(); j++ ) 
{ 
DBoW3::BowVector v2; 
vocab.transform( descriptors[j], v2 ); 
double score = vocab.score(vi, v2); 
cout<<"image "<<i<<" vs image "<<j<<" : "<<score<<endl; 
} 
cout<<endl; 
} 


// or with database 
cout<<"comparing images with database "<<end1; 
DBoW3::Database db( vocab, false, 0); 
for ( int i=0; i<descriptors.size(); i++ ) 
db.add(descriptors[i]); 
cout<<"database info: "<<db<<endl; 
for ( int i=0; i<descriptors.size(); i++ ) 
{ 
DBoW3::QueryResults ret; 
db.query( descriptors[i], ret, 4); // max result=4 


cout<<"searching for image "<<i<<" returns "<<ret<<endl<<end1; 


据 库 之 间 的 比较 


60 J 


61 cout<<"done."<<end1; 


本 程序 演示 了 两 种 比 对 方式 : 图 像 之 间 的 直接 比较 ， 以 及 图 像 与 数 
尽管 它们 是 大 同 小 异 的 。 此 外 ， 我 们 输出 了 每 幅 图 


像 对 应 的 Bag-of-Words 描 述 向 量 ， 读 者 可 以 从 输出 数据 中 看 到 它们 。 


1 |$ build/feature_training 

2 |reading database 

3 | reading images... 

4 |detecting ORB features ... 

5 | comparing images with images 

6 |desp 0 size: 500 

7 | transform image 0 into BoW vector: size = 455 

8 | key value pair = <1, 0.00155622>, <3, 0.00222645>, <12, 0.00222645>, <13, 
0.00222645>, <14, 0.00222645>, <22, 0.00222645>, <33, 0.00222645>, <37, 
0.00155622>, <38, 0.00222645>, <39, 0.00222645>, <43, 0.00222645>, <57, 0.00155622> 


可 以 看 到 ，Bow 描 述 向 量 中 含有 每 个 单词 的 ID 和 权重 ， 它 们 构成 了 


整个 稀 下 的 向 量 。 当 我 们 比较 两 个 向 量 时 ，DBoW3 会 为 我 们 计算 一 个 分 
数 ， 计 算 的 方式 由 之 前 构造 字典 时 定义 : 


果 : 


1 | image 0 vs image 0 : 1 

2 | image 0 vs image 1 : 0.0234552 
3 | image 0 vs image 2 : 0.0225237 
4 | image 0 vs image 3 : 0.0254611 
5 | image 0 vs image 4 : 0.0253451 
6 | image 0 vs image 5 : 0.0272257 
7 | image 0 vs image 6 : 0.0217745 
s | image 0 vs image 7 : 0.0231948 
9 | image 0 vs image 8 : 0.0311284 
10 | image 0 vs image 9 : 0.0525447 


在 数据 库 查 询 时 ，DBoW 对 上 面 的 分 数 进 行 排序 ， 给 出 最 相似 的 结 


searching for image 0 returns 4 results: 
<EntryId: 0, Score: 1> 

<EntryId: 9, Score: 0.0525447> 

<EntryId: 8, Score: 0.0311284> 

<EntryId: 5, Score: 0.0272257> 


searching for image 1 returns 4 results: 
<EntryId: 1, Score: 1> 
<EntryId: 2, Score: 0.0339641> 


<EntrylId: 
<EntrylId: 


searching 
<EntrylId: 
<EntrylId: 
<EntryId: 
<EntrylId: 


searching 
<EntrylId: 
<EntrylId: 
<EntrylId: 
<EntryId: 


searching 
<EntryId: 
<EntrylId: 
<EntrylId: 
<EntrylId: 


searching 
<EntrylId: 
<EntrylId: 
<EntrylId: 
<EntrylId: 


searching 
<EntrylId: 
<EntrylId: 
<EntrylId: 
<EntrylId: 


searching 
<EntrylId: 
<EntryId: 
<EntrylId: 
<EntrylId: 


searching 
<Entryld: 
<EntrylId: 
<EntrylId: 
<Entryld: 


8, Score: 


3, Score: 


for image 


2, Score: 
7, Score: 
9, Score: 


1, Score: 


for image 


3, Score: 
9, Score: 
8, Score: 


5, Score: 


for image 


4, Score: 
5, Score: 
0, Score: 


6, Score: 


for image 


5, Score: 
4, Score: 
9, Score: 


6, Score: 


for image 


6, Score: 
8, Score: 
5, Score: 


3, Score: 


for image 


7, Score: 
2, Score: 
1, Score: 


0, Score: 


for image 


8, Score: 
9, Score: 
0, Score: 


6, Score: 


0.0299387> 
0.0256668> 


2 returns 4 


1> 
0.036092> 
0.0348702> 
0.0339641> 


3 returns 4 


1> 

0.0357317> 
0.0278496> 
0.0270168> 


4 returns 4 


1> 

0.0493492> 
0.0253451> 
0.0253017> 


5 returns 4 


1> 
0.0493492> 
0.028996> 
0.0277584> 


6 returns 4 


1> 

0.0306241> 
0.0277584> 
0.0267135> 


7 returns 4 


1> 
0.036092> 
0.0239091> 
0.0231948> 


8 returns 4 


1> 

0.0329149> 
0.0311284> 
0.0306241> 


results: 


results: 


results: 


results: 


results: 


results: 


results: 


55 | Searching for image 9 returns 4 results: 


on 
a 


<EntryId: 9, Score: 1> 

57 |<EntryId: 0, Score: 0.0525447> 
ss |<EntryId: 3, Score: 0.0357317> 
<EntryId: 2, Score: 0.0348702> 


w 
© 


读者 可 以 查看 所 有 的 输出 ， 看 看 不 同 图 像 与 相似 图 像 评 分 有 多 大 差 
异 。 我 们 看 到 ， 明 显 相似 的 图 1 和 图 10 〈 在 C++ 中 下 标 为 分 别 为 0 和 9) ， 
其 相似 度 评分 约 0.0525; 而 其 他 图 像 约 为 0.02。 


在 本 节 的 演示 实验 中 ， 我 们 看 到 相似 图 像 1 和 10 的 评分 明显 高 于 其 他 
图 像 对 ， 然 而 就 数值 上 看 并 没有 我 们 想象 的 那么 明显 。 按 说 如 果 自 己 和 
自己 比较 相似 度 为 100%， 那 么 我 们 (从 人 类 角度 ) 认为 图 1 和 图 10 至 少 也 
有 百 分 之 七 八 十 的 相似 度 ， 而 其 他 图 可 能 为 百 分 之 二 三 十 。 然 而 实验 结 
果 却 是 无 关 图 像 约 2%， 相 似 图 像 约 5%， 似 乎 没有 我 们 想象 的 那么 明显 。 
这 是 否 是 我 们 想 要 看 到 的 结果 呢 ? 


125 ”实验 分 析 与 评述 
12.5.1 ”增加 字典 规模 


在 机 器 学 习 领 域 ， 如 果 代 码 没 有 出 错 而 结果 不 满意 ， 我 们 首先 怀疑 
“网 络 结构 是 否 够 大 ， 层 数 是 否 足够 深 ， 数 据 样 本 是 否 够 多 ”等 ， 这 依然 
是 出 于 “好 模型 敌 不 过 烂 数 据 ” 的 大 原则 (一 方面 也 是 因为 缺乏 更 深层 次 
的 理论 分 析 ) 。 尽 管 我 们 现在 是 在 研究 SLAM， 但 出 现 这 种 情况 ， 我 们 首 
先 会 怀疑 : 是 不 是 字典 选 得 太 小 了 ? 毕竟 我 们 是 从 十 幅 图 生成 的 字典 ， 
然后 又 根据 这 个 字典 来 计算 图 像 相 似 性 。 

slambook/ch9/vocab_larger.yml.gz 是 我 们 生成 的 一 个 稍微 大 一 点 儿 的 
字典 一 一 事实 上 是 对 同一 个 数据 序列 的 所 有 图 像 生 成 的 ， 大 约 有 2,900 幅 
图 像 。 字 典 的 规模 仍然 取 k =10,d =5， 即 最 多 一 万 个 单词 。 读 者 可 以 使 用 
同 目录 下 的 gen_vocab_large.cpp 文 件 自行 训练 字典 。 请 注意 ， 若 要 训练 大 
型 了 字典， 可 能 需要 一 人 台 内 人 存 较 大 的 机 器 ， 并 且 了 耐心 等 上 一 段 时 间 。 我 们 
对 上 一 节 的 程序 稍 加 修改 ， 使 用 更 大 的 字典 去 检测 图 像 相似 性 : 


comparing images with database 


database info: Database: Entries = 10, Using direct index = no. Vocabulary: k = 10, 


L = 5, Weighting = 
searching for image 
<EntryId: 
<EntrylId: 
<EntryId: 
<EntrylId: 


searching for image 


0, Score: 
9, Score: 
8, Score: 


4, Score: 


tf-idf, Scoring = Li-norm, Number of words = 99566 
0 returns 4 results: 

1> 

0.0320906> 

0.0103268> 

0.0066729> 


1 returns 4 results: 


<EntryId: 
<EntryId: 
<EntryId: 
<EntryId: 


searching 
<EntryId: 
<EntryId : 
<EntryId : 
<EntrylId: 


searching 
<EntryId: 
<EntrylId: 
<EntryId: 
<Entryld: 


searching 
<EntrylId: 
<EntrylId: 
<EntrylId: 
<EntrylId: 


searching 
<EntrylId: 
<EntryId: 
<Entryld: 
<EntrylId: 


searching 
<Entryld: 
<EntrylId: 
<EntrylId: 
<EntryId: 


searching 
<Entryld: 
<Entryld: 
<Entryld: 
<Entryld: 


searching 
<EntrylId: 
<Entryld: 
<EntryId: 


for image 
2, Score: 
1, Score: 
5, Score: 


8, Score: 


for image 
3, Score: 
5, Score: 
8, Score: 


6, Score: 


for image 
4, Score: 
6, Score: 
0, Score: 


5, Score: 


for image 
5, Score: 
3, Score: 
2, Score: 


4, Score: 


for image 
6, Score: 
7, Score: 
3, Score: 


4, Score: 


for image 
7, Score: 
6, Score: 
8, Score: 


1, Score: 


for image 
8, Score: 
0, Score: 


2, Score: 


1> 


: 0.0238409> 
: 0.00814409> 


0.00697527> 


2 returns 4 
1> 

0.0238409> 
0.00897928> 
0.00893477> 


3 returns 4 
1> 

0.0107005> 
0.00870392> 
0.00720695> 


4 returns 4 
1> 

0.0069998> 
0.0066729> 
0.0062834> 


5 returns 4 
1> 

0.0107005> 
0.00897928> 
0.0062834> 


6 returns 4 
1> 

0.00915307> 
0.00720695> 
0.0069998> 


7 returns 4 
1> 

0.00915307> 
0.00814517> 
0.00538609> 


8 returns 4 
1> 

0.0103268> 
0.00893477> 


results: 


results: 


results: 


results: 


results: 


results: 


results: 


55 |《EntryId: 3, Score: 0.00870392> 


5 | searching for image 9 returns 4 results: 
ss |<EntryId: 9, Score: 1> 

59 |<EntryId: 0, Score: 0.0320906> 

6 |<EntryId: 8, Score: 0.00636511> 

61 |<EntryId: 1, Score: 0.00587605> 


可 以 看 到 ， 当 字典 规模 增加 时 ， 无关 图 像 的 相似 性 明显 变 小 了 。 而 
相似 的 图 像 ， 例 如 图 像 1 和 10， 虽 然 分 值 也 略微 下 降 ， 但 相对 于 其 他 图 像 
的 评分 ， 却 变 得 更 为 显著 了 。 这 说 明 增 加 字典 训练 样本 是 有 蔓 的 。 同 
理 ， 读 者 可 以 党 试 使 用 更 大 规模 的 字典 ， 看 看 结果 会 发 生 怎样 的 变化 。 


12.5.2 ”相似 性 评分 的 处 理 


对 任意 两 幅 图 像 ， 我 们 都 能 给 出 一 个 相似 性 评分 ， 但 是 只 利用 这 个 
分 值 的 绝对 大 小 并 不 一 定 有 很 好 的 帮助 。 比 如 说 ， 有 些 环 境 的 外 观 本 来 
就 很 相似 ， 像 办 公 室 往往 有 很 多 同 款式 的 桌 椅 ; 另 一 些 环境 则 各 个 地 方 
都 有 很 大 的 不 同 。 考 虑 到 这 种 情况 ， 我 们 会 取 一 个 先 验 相似 度 s (v, v. a, 
)， 它 表示 某 时 刻 关 键 帧 图 像 与 上 一 时 刻 的 关键 帧 的 相似 性 。 然 后 ， 其 他 
的 分 值 都 参照 这 个 值 进行 归 一 化 : 


s (vi, vi) = s (vi, ve,) /s (Vt, Vt_At). (12.10) 


站 在 这 个 角度 上 ， 我 们 说 : 如 果 当 前 帧 与 之 前 某 关键 帧 的 相似 度 起 
过 当前 帧 与 上 一 个 关键 帧 相似 度 的 3 倍 ， 就 认为 可 能 存在 回环 。 这 个 步 又 
避免 了 引入 绝对 的 相似 性 阅 值 ， 使 得 算法 能 够 适应 更 多 的 环境 。 


12.5.3 ”关键 帧 的 处 理 


在 检测 回环 时 ， 我 们 必须 考虑 到 关键 帧 的 选取 。 如 果 关 键 帧 选 得 太 
近 ， 那 么 将 导致 两 个 关键 帧 之 间 的 相似 性 过 高 ， 相 比 之 下 不 容易 检测 出 
历史 数据 中 的 回环 。 比 如 ， 检 测 结果 经 常 是 第 n 帧 和 第 n- 2 帧 、 第 m 3 帧 
最 为 相似 ， 这 种 结果 似乎 太平 凡 了 ， 意 义 不 大 。 所 以 从 实践 上 说 ， 用 于 


回环 检测 的 帧 最 好 是 稀 臣 一 些 ， 役 此 之 间 不 太 相 同 ， 又 能 涵 兰 整个 环 
境 。 

另 一 方面 ， 如 果 成 功 检 测 到 了 回环 ， 比 如 说 出 现在 第 1 帧 和 第 mn” 帧 。 
那么 很 可 能 第 n +1 帧 、 第 mn +2 帧 都 会 和 第 1 帧 构成 回环 。 但 是 ， 确 认 第 1 帧 
和 第 n 帧 之 间 存 在 回环 对 轨迹 优化 是 有 帮助 的 ， 而 再 接 下 去 的 第 n +1 帧 、 
Bn +2 帧 都 会 和 第 1 帧 构成 回环 产生 的 帮助 就 没 那 么 大 了 ， 因 为 我 们 已 经 
用 之 前 的 信息 消除 了 累积 误差 ， 更 多 的 回环 并 不 会 带 来 更 多 的 信息 。 所 
以 ， 我 们 会 把 “相近 ”的 回环 聚 成 一 类 ， 使 算法 不 要 反复 地 检测 同一 类 的 
回环 。 


12.5.4 ”检测 之 后 的 验证 


词 袋 的 回环 检测 算法 完全 依赖 于 外 观 而 没有 利用 任何 的 几何 信息 ， 
这 导致 外 观 相似 的 图 像 容易 被 当成 回环 。 并 且 ， 由 于 词 袋 不 在 乎 单词 顺 
序 ， 只 在 意 单词 有 无 的 表达 方式 ， 更 容易 引发 感知 偏差 。 所 以 ， 在 回环 
检测 之 后 ， 我 们 通常 还 会 有 一 个 验证 步骤 ?03 。 


验证 的 方法 有 很 多 。 其 一 是 设立 回环 的 缓存 机 制 ， 认 为 单 次 检测 到 
的 回环 并 不 足以 构成 民 好 的 约束 ， 而 在 一 段 时 间 中 一 直 检 测 到 的 回环 ， 
才 认 为 是 正确 的 回环 。 这 可 以 看 成 时 间 上 的 一 致 性 检测 。 另 一 方法 是 空 
间 上 的 一 致 性 检测 ， 即 对 回环 检测 到 的 两 个 帧 进行 特征 匹配 ， 估 计 相 机 
的 运动 。 然 后 ， 再 把 运动 放 到 之 前 的 Pose Graph 中 ， 检 查 与 之 前 的 估计 是 
否 有 很 大 的 出 入 。 总 之 ， 验 证 部 分 通常 是 必需 的 ， 但 如 何 实现 却 是 见 仁 
见 智 的 问题 。 


12.5.5 “与 机 器 学 习 的 关系 


从 前 面 的 论述 中 可 以 看 出 ， 回 环 检测 与 机 器 学 习 有 着 干 丝 万 缕 的 天 
联 。 回 环 检测 本 身 非常 像 是 一 个 分 类 问题 。 与 传统 模式 识别 的 区 别 在 
于 ， 回 环 中 的 类 别 数量 很 大 ， 而 每 类 的 样本 很 少 一 一 极端 情况 下 ， 当 机 
器 人 发 生 运动 后 ， 图 像 发 生变 化 ， 就 产生 了 新 的 类 别 ， 我 们 甚至 可 以 把 
类 别 当 成 连续 变量 而 非 离散 变量 ; 而 回环 检测 ， 相 当 于 两 幅 图 像 落 入 同 
一 类 ， 则 是 很 少 出 现 的 。 从 另 一 个 角度 看 ， 回 环 检 测 也 相当 于 对 “图 像 间 


相似 性 ”概念 的 一 个 学 习 。 既 然 人 类 能 够 掌握 图 像 是 否 相 似 的 判断 ， 让 机 
器 学 习 到 这 样 的 概念 也 是 非常 有 可 能 的 。 

从 词 袋 模型 来 说 ， 它 本 身 是 一 个 非 监督 的 机 器 学 习 过 程 一 一 构建 词 
典 相当 于 对 特征 描述 子 进行 聚 类 ， 而 树 只 是 对 所 聚 的 类 的 一 个 快速 查找 
的 数据 结构 而 已 。 既 然 是 聚 类 ， 结 合 机 器 学 习 里 的 知识 ， 我 们 至 少 可 以 
问 : 

1. 是 否 能 对 机 器 学 习 的 图 像 特征 进行 聚 类 ， 而 不 是 对 SURF、ORB 这 
样 的 人 工 设计 特征 进行 聚 类 ? 

2. 是 否 有 更 好 的 方式 进行 聚 类 ， 而 不 是 用 树 结构 加 上 K-means 这 些 较 
朴素 的 方式 ? 

结合 目前 机 器 学 习 的 发 展 ， 二 进 制 描述 子 的 学 习 和 无 监督 的 聚 类 ， 
都 是 非常 有 望 在 深度 学 习 框架 中 得 以 解决 的 问题 。 我 们 也 陆续 看 到 了 利 
用 机 器 学 习 进 行 回环 检测 的 工作 。 尽 管 目前 词 袋 方法 仍 是 主流 ， 但 笔者 
本 人 相信 ， 未 来 深度 学 习 方 法 很 有 和 希望 打败 这 些 人 工 设 计 特 征 的 、“ 传 
SANNA FIA) 。 上 毕竟 词 袋 方法 在 物体 识别 问题 上 已 经 明显 不 
如 神经 网 络 了 ， 而 回环 检测 又 是 非常 相似 的 一 个 问题 。 

习题 

1. 请 书写 计算 PR 曲线 的 小 程序 。 用 MAILAB 或 Python 可 能 更 加 简便 
一 些 ， 因 为 它们 擅长 作 图 。 

2. 验 证 回环 检测 算法 ， 需 要 有 人 工 标记 回环 的 数据 集 ， 例 如 [94]。 然 
而 人 工 标 记 回环 是 很 不 方便 的 ， 我 们 会 考虑 根据 标准 轨迹 计算 回环 。 
即 ， 如 果 轨 人 迹 中 有 两 个 帧 的 位 姿 非 常 相近 ， 就 认为 它们 是 回环 。 请 根据 
TUM 数 据 集 给 出 的 标准 轨迹 ， 计 算出 一 个 数据 集中 的 回环 。 这 些 回环 的 
图 像 真 的 相似 吗 ? 

3. 学 习 DBoW3 或 DBoW2 库 ， 自 己 寻 找 几 张 图 片 ， 看 能 否 从 中 正确 检 
测 出 回环 。 

4. 调 研 相似 性 评分 的 常用 度量 方式 ， 哪 些 比较 常用 ? 

5.Chow-Liu 树 是 什么 原理 ? 它 是 如 何 被 用 于 构建 字典 和 回环 检测 的 ? 

6. 阅 读 文献 [106]， 除 了 词 袋 模型 ， 还 有 哪些 用 于 回环 检测 的 方法 ? 


[1] 有 机 器 学 习 背 景 的 读者 ， 应 该 能 感受 出 这 段 话 与 机 器 学 习 是 何等 相似 。 你 是 不 是 已 经 在 想 如 何 
训练 网 络 了 呢 ? 


[2] 虽然 这 种 性 质 有 时 也 会 带 来 一 些 问题 ， 例 如 ， 眼 睛 长 在 嘴巴 下 的 脸 仍 是 人 脸 吗 ? 
[3] 你 会 把 一 页 只 有 十 个 单词 的 纸 叫 作 字典 吗 ? 笔者 相信 大 多 数 人 心目 中 的 字典 都 是 相当 厚重 的 。 


[4] 我 们 用 了 XK 和 d 表达 树 的 分 支 和 深度 ， 这 可 能 会 令 你 想到 k-d 树 名 。 笔 者 觉得 虽然 做 法 不 尽 相 
同 ， 但 它们 表达 的 含义 确实 是 一 致 的 。 


[5] 选用 它 的 主要 原因 是 其 对 OpenCV3 兼 容 性 较 好 ， 且 编译 和 使 用 都 容易 上 手 。 
[6] 这 里 的 L 即 前 文 说 的 d 。 
[7] 个 人 觉得 TF-IDF 称 呼 起 来 更 顺口 ， 所 以 后 文 就 用 英文 缩写 而 非 中文 译 文 了 。 


第 13 讲 ” 建 图 


主要 目标 
1. 理 解 单 目 SLAM 中 稠密 深度 估计 的 原理 。 


2. 通 过 实验 了 解 单 目 稠密 重建 的 过 程 。 
3. 了 解 几 种 RGB-D 重 建 中 的 地 图 形式 。 


本 讲 我 们 开始 介绍 建 图 部 分 的 算法 。 在 前 端 和 后 端 中 ， 我 们 重点 天 
注 同 时 估计 相机 运动 轨迹 与 特征 点 空间 位 置 的 问题 。 然 而 ， 在 实际 使 用 
SLAM 时 ， 除 了 对 相机 本 体 进行 定位 之 外 ， 还 存在 许多 其 他 的 需求 。 例 
如 ， 考 虑 放 在 机 器 人 上 的 SLAM， 那 么 我 们 会 希望 地 图 能 够 用 于 定位 、 导 
航 、 避 障 和 交互 ， 特 征 点 地 图 显然 不 能 满足 所 有 的 这 些 需 求 。 所 以 ， 本 
讲 我 们 将 更 详细 地 讨论 各 种 形式 的 地 图 ， 并 指出 目前 视觉 SLAM 地 图 中 和 存 
在 着 的 缺陷 。 


13.1 概述 


建 图 (Mapping) ， 本 应 该 是 SLAM 的 两 大 目标 之 一 一 一 因为 SLAM 
被 称 为 同时 定位 与 建 图 。 但 是 直到 现在 ， 我 们 讨论 的 都 是 定位 问题 ， 包 
括 通过 特征 点 的 定位 、 直 接 法 的 定位 ， 以 及 后 端 优 化 。 那 么 ， 这 是 否 暗 
示 建 图 在 SLAM 里 没有 那么 重要 ， 所 以 我 们 直到 本 讲 才 开始 讨论 呢 ? 

答案 是 否定 的 。 事 实 上 ， 在 经 典 的 SLAM 模 型 中 ， 我 们 所 谓 的 地 图 ， 
即 所 有 路 标点 的 集合 。 一 旦 确定 了 路 标点 的 位 置 ， 那 就 可 以 说 我 们 完成 
了 建 图 。 于 是 ， 前 面 说 的 视觉 里 程 计 也 好 ，Bundle Adjustment 也 好 ， 事 实 
上 都 建 模 了 路 标点 的 位 置 ， 并 对 它们 进行 优化 。 从 这 个 角度 上 说 ， 我 们 
已 经 探讨 了 建 图 问题 。 那 么 为 何 还 要 单独 列 一 讲 建 图 呢 ? 


这 是 因为 人 们 对 建 图 的 需求 不 同 。SLAM 作 为 一 种 底层 技术 ， 往 往 是 
用 来 为 上 层 应 用 提供 信息 的 。 如 果 上 层 是 机 器 人 ， 那 么 应 用 层 的 开发 者 
可 能 希望 使 用 SLAM 来 做 全 局 的 定位 ， 并 且 让 机 器 人 在 地 图 中 导航 一 一 例 
如 扫地 机 需要 完成 扫地 工作 ， 希 望 计算 一 条 能 够 履 盖 整 张 地 图 的 路 径 。 


或 者 ， 如 果 上 层 是 一 个 增强 现实 设备 ， 那 么 开发 者 可 能 希望 将 虚拟 物体 
合 加 在 现实 物体 之 中 ， 特 别 地 ， 还 可 能 需要 处 理 虚拟 物体 和 真实 物体 的 
遮挡 天 系 。 


我 们 发 现 ， 应 用 层面 对 于 “定位 ”的 需求 是 相似 的 ， 它 们 希望 SLAM 提 
供 相 机 或 搭载 相机 的 主体 的 空间 位 姿 信 息 。 而 对 于 地 图 ， 则 存在 着 许多 
不 同 的 需求 。 在 视觉 SLAM 看 来 ,“ 建 图 ”是 服务 于 “定位 ”的 ;) 但 是 在 应 用 
层面 看 来 ,“ 建 图 ”明显 还 市 有 许多 其 他 的 需求 。 关 于 地 图 的 用 处 ， 我 们 
大 致 归纳 如 下 : 


1. 定 位 。 定 位 是 地 图 的 一 项 基本 功能 。 在 前 面 的 视觉 里 程 计 部 分 ， 
我 们 讨论 了 如 何 利 用 局 部 地 图 来 实现 定位 。 在 回环 检测 部 分 ， 我 们 也 看 
到 ， 只 要 有 全 局 的 描述 子 信息 ， 我 们 也 能 通过 回环 检测 确定 机 器 人 的 位 
置 。 更 进一步 ， 我 们 还 希望 能 够 把 地 图 保存 下 来 ， 让 机 器 人 在 下 次 开机 
后 依然 能 在 地 图 中 定位 ， 这 样 只 需 对 地 图 进行 一 次 建 模 ， 而 不 是 每 次 启 
动机 器 人 都 重新 做 一 次 完整 的 SLAM。 


2. 导 航 。 导 航 是 指 机 器 人 能 够 在 地 图 中 进行 路 径 规 划 ， 在 任意 两 个 
地 图 点 间 寻 找 路 径 ， 然 后 控制 自己 运动 到 目标 点 的 过 程 。 该 过 程 中 ， 我 
们 至 少 需 要 知道 地 图 中 哪些 地 方 不 可 通过 ， 而 哪些 地 方 是 可 以 通过 的 。 
这 融 超 出 了 稀 足 特征 点 地 图 的 能 力学 围 ， 我 们 必须 有 另外 的 地 图 形式 。 
稍 后 我 们 会 说 ， 这 至 少 得 是 一 种 稠密 的 地 图 。 


3. 避 障 。 避 障 也 是 机 器 人 经 常 磁 到 的 一 个 问题 。 它 与 导航 类 似 ， 但 
更 注重 局 部 的 、 动 态 的 障碍 物 的 处 理 。 同 样 ， 仅 有 特征 点 ， 我 们 无 法 判 
断 某 个 特征 点 是 否 为 障碍 物 ， 所 以 需要 稠密 地 图 。 


4. 重 建 。 有 时候， 我 们 希望 利用 SLAM 获 得 周围 环境 的 重建 效果 ， 并 
把 它 展 示 给 其 他 人 看 。 这 种 地 图 主要 用 于 向 人 展示 ， 所 以 我 们 希望 它 看 
上 去 比较 舒服 、 美 观 。 或 者 ， 我 们 也 可 以 把 该 地 图 用 于 通信 ， 使 其 他 人 
能 够 远程 地 观看 我 们 重建 得 到 的 三 维 物体 或 场景 一 一 例如 三 维 的 视频 通 
话 或 者 网 上 购物 等 。 这 种 地 图 亦 是 稠密 的 ， 并 且 我 们 还 对 它 的 外 观 有 一 
些 要 求 。 我 们 可 能 不 满足 于 稠密 点 云 重建 ， 更 希望 能 够 构建 带 纹理 的 平 
面 ， 就 像 电子 游戏 中 的 三 维 场景 那样 。 


5. 交 互 。 交 互 主要 指 人 与 地 图 之 间 的 互动 。 例 如 ， 在 增强 现实 中 ， 
我 们 会 在 房间 里 放置 虚拟 的 物体 ， 并 与 这 些 虚拟 物体 之 间 有 一 些 互 动 
一 一 比方 说 我 会 点 击 墙 面 上 放 着 的 虚拟 网 页 浏览 器 来 观看 视频 ， 或 者 向 
墙 面 投掷 物体 ， 和 希望 它们 有 (虚拟 的 ) 物理 碰撞 。 另 一 方面 ， 机 器 人 应 


用 中 也 会 有 与 人 、 与 地 图 之 间 的 交互 。 例 如 ， 机 器 人 可 能 会 收 到 命令 “ 取 
昌 子 上 的 报纸 ”"， 那 么 ， 除 了 有 环境 地 图 之 外 ， 机 器 人 还 需要 知道 哪 一 块 
地 图 是 “桌子 *”， 什 么 叫 作 “之 上 ”， 什 么 又 叫 作 “报纸 "。 这 需要 机 器 人 对 地 
图 有 更 高 层面 的 认 知 一 一 亦 称 为 语义 地 图 。 


图 13-1 形 象 地 解释 了 上 面 讨 论 的 各 种 地 图 类 型 与 用 途 之 间 的 关系 。 我 
们 之 前 的 讨论 ， 基 本 集中 于 “ 稀 下 路 标 地 图 ”部 分 ， 还 没有 探讨 稠密 地 
图 。 所 谓 稠 密 地 图 是 相对 于 稀 踊 地 图 而 言 的 。 稀 疏 地 图 只 建 模 感 兴趣 的 
部 分 ， 也 就 是 前 面 说 了 很 久 的 特征 点 (路标 点) 。 而 稠密 地 图 是 指 ， 建 
模 所 有 看 到 过 的 部 分 。 对 于 同一 张 桌子 ， 稀 疏 地 图 可 能 只 建 模 了 桌子 的 
四 个 角 ， 而 稠密 地 图 则 会 建 模 整个 桌面 。 虽 然 从 定位 角度 看 ， 只 有 四 个 
角 的 地 图 也 可 以 用 于 对 相机 进行 定位 ， 但 由 于 我 们 无 法 从 四 个 角 推 断 这 
几 个 点 之 间 的 空间 结构 ， 所 以 无 法 仅 用 四 个 角 来 完成 导航 、 避 障 等 需要 
稠密 地 图 才能 完成 的 工作 。 


图 13-1 各 种 地 图 的 示意 图 。 三 种 例子 地 图 分 别 来 自 文献 [73,107,108]。 


从 上 面 的 讨论 中 可 以 看 出 ， 稠 密 地 图 占据 着 一 个 非常 重要 的 位 置 。 
于 是 ， 剩 下 的 问题 是 : 通过 视觉 SLAM 能 建立 稠密 地 图 吗 ? 如 果 能 ， 怎 么 


建 呢 ? 


13.2 单 目 稠密 重建 
13.2.1 ”立体 视觉 


视觉 SLAM 的 稠密 重建 问题 是 本 讲 的 第 一 个 重要 话题 。 相 机 ， 很 久 以 
来 被 认为 是 只 有 角度 的 传感器 (Bearing only) 。 单 幅 图 像 中 的 像素 ， 只 
能 提供 物体 与 相机 成 像 平 面 的 角度 及 物体 采集 到 的 亮度 ， 而 无 法 提供 物 
体 的 距离 (Range) 。 而 在 稠密 重建 中 ， 我 们 需要 知道 每 一 个 像素 点 (或 
大 部 分 像素 点 ) 的 距离 ， 对 此 大 致 上 有 如 下 解决 方案 : 


1. 使 用 单 目 相 机 ， 通 过 移动 相机 之 后 进行 三 角 化 测量 像素 的 距离 。 


2. 使 用 双 目 相机 ， 利 用 左右 目的 视差 计算 像素 的 距离 (多 目 原 理 相 
同 ) 。 


3. 使 用 RGB-D 相 机 直接 获得 像素 距离 。 


前 两 种 方式 称 为 立体 视觉 (Stereo Vision) ， 其 中 移动 单 目 的 又 称 为 
移动 视角 的 立体 视觉 (Moving View Stereo) 。 相 比 于 RGB-D 直 接 测 量 的 
深度 ， 单 自 和 双 目 对 深度 的 获取 往往 是 “费力 不 讨好 ”的 一 一 我 们 需要 花 
费 大 量 的 计算 ， 最 后 得 到 一 些 不 怎么 可 靠 的 趾 深 度 估计 。 当 然 ，RGB-D 
也 有 一 些 量 程 、 应 用 范围 和 光照 的 限制 ， 不 过 相 比 于 单 目 和 双 目 的 结 
果 ， 使 用 RGB-D 进 行 稠密 重建 往往 是 更 常见 的 选择 。 而 单 目 、 双 目的 好 
处 是 ， 在 目前 RGB-D 还 无 法 很 好 应 用 的 室外 、 大 场景 场合 中 ， 仍 能 通过 
立体 视觉 估计 深度 信息 。 

话 虽 如 此 ， 本 节 我 们 将 带领 读者 实现 一 遍 单 目的 稠密 估计 ， 体 验 为 
何 说 它 是 费力 不 讨好 的 。 我 们 从 最 简单 的 情况 开始 说 起 : 在 给 定 相机 轨 
迹 的 基础 上 ， 如 何 根据 一 段 时 间 的 视频 序列 来 估计 某 幅 图 像 的 深度 。 换 
言 之 ， 我 们 不 考虑 SLAM， 先 来 考虑 略为 简单 的 建 图 问题 。 


假定 有 一 段 视频 序列 ， 我 们 通过 某 种 魔法 得 到 了 每 一 帧 对 应 的 轨迹 
(当然 也 很 可 能 是 由 视觉 里 程 计 前 端 估 计 所 得 ) 。 现 在 我 们 以 第 一 幅 图 
像 为 参考 帧 ， 计 算 参 考 帧 中 每 一 个 像素 的 深度 (或 者 说 距离 o HI, 
请 回忆 在 特征 点 部 分 我 们 是 如 何 完成 该 过 程 的 : 


1. 首 先 ， 我 们 对 图 像 提 取 特 征 ， 并 根据 换 述 子 计 算 了 特征 之 间 的 匹 
配 。 换 言 之 ， 通 过 特征 ， 我 们 对 某 一 个 空间 点 进行 了 跟踪 ， 知道 了 它 在 
各 个 图 像 之 间 的 位 置 。 


过 不 同 视 角 下 的 观测 估计 它 的 深 度 ， 原 理 即 前 面 讲 过 的 三 角 测量 。 


在 稠密 深度 图 估计 中 ， 不 同 之 处 在 于 ， 我 们 无 法 把 每 个 像素 都 当 作 
特征 点 计算 描述 子 。 因 此 ， 竺 密 深度 估计 问题 中 ， 匹 配 就 成 为 很 重要 的 
一 环 : 如 何 确 定 第 一 幅 图 的 某 像素 出 现在 其 他 图 里 的 位 置 呢 ? 这 需要 用 
到 极 线 搜索 和 块 匹配 技术 "9 。 然 后 ， 当 我 们 知道 了 某 个 像素 在 各 个 图 
中 的 位 置 ， 就 能 像 特征 点 那样 ， 利 用 三 角 测 量 确 定 它 的 深度 。 不 过 不 同 
的 是 ， 在 这 里 我 们 要 使 用 很 多 次 三 角 测量 让 深度 估计 收敛 ， 而 不 仪 是 一 
次 。 我 们 希望 深度 估计 能 够 随 着 测量 的 增加 从 一 个 非常 不 确定 的 量 ， 逐 
渐 收敛 到 一 个 稳定 值 。 这 就 是 深度 滤波 器 技术 。 所 以 ， 下 面 的 内 容 将 主 
要 围绕 这 个 主题 展开 。 


13.2.2 ” 极 线 搜索 与 块 匹 配 


我 们 先 来 探讨 不 同 视角 下 观察 同一 个 点 产生 的 几何 关系 。 这 非常 像 
在 第 7.3 节 讨论 的 对 极 几 何 关 系 。 请 看 图 13-2。 左 边 的 相机 观测 到 了 某 个 
像素 p , 。 由 于 这 是 一 个 单 目 相机 ， 我 们 无 从 知道 它 的 深度 ， 所 以 假设 这 
个 深度 可 能 在 某 个 区 域 之 内 ， 不 妨 说 是 某 最 小 值 到 无 穷 远 之 间 : (d,,,, +oo 
)。 因 此 ， 该 像素 对 应 的 空间 点 就 分 布 在 某 条 线段 (本 例 中 是 射线 ) 上 。 
在 另 一 个 视角 〈 右 侧 相 机 ) 看 来 ， 这 条 线段 的 投影 也 形成 图 像 平 面 上 的 
一 条 线 ， 我 们 知道 这 称 为 极 线 。 当 知道 两 部 相机 间 的 运动 时 ， 这 条 极 线 
也 是 能 够 确定 的 2 。 那 么 问题 就 是 : 极 线 上 的 哪 一 个 点 是 我 们 刚才 看 到 
Bp , 点 呢 ? 


图 13-2” 极 线 搜索 示意 图 。 


重复 一 亿 ， 在 特征 点 方法 中 ， 我 们 通过 特征 匹配 找到 了 p , 的 位 置 。 
然而 现在 我 们 没有 描述 子 ， 所 以 只 能 在 极 线 上 搜索 和 p , 长 得 比较 相似 的 
点 。 再 具体 地 说 ， 我 们 可 能 疝 着 第 二 幅 图 像 中 的 极 线 的 某 一 头 走 到 另 一 
头 ， 逐 个 比较 每 个 像素 与 p , 的 相似 程度 。 从 直接 比较 像素 的 角度 来 看 ， 
这 种 做 法 倒是 和 直接 法 是 异曲同工 的 。 

在 直接 法 的 讨论 中 我 们 知道 ， 比 较 单个 像素 的 亮度 值 并 不 一 定 稳定 
可 靠 。 一 件 很 明显 的 事情 就 是 : 万 一 极 线 上 有 很 多 和 p AMR, RI] 
怎么 确定 哪 一 个 是 真实 的 呢 ? 这 似乎 回 到 了 我 们 在 回环 检测 当中 说 到 的 
问题 : 如 何 确定 两 幅 图 像 (或 两 个 点 ) 的 相似 性 ? 回环 检测 是 通过 词 袋 
来 解决 的 ， 但 这 里 由 于 没有 特征 ， 所 以 只 好 寻求 另外 的 途径 。 

一 种 直观 的 想法 是 : 既然 单个 像素 的 亮度 没有 区 分 性 ， 那 是 否 可 以 
比较 像素 块 呢 ? 我 们 在 p , 周围 取 一 个 大 小 为 wxw 的 小 块 ， 然 后 在 极 线 上 


也 取 很 多 同样 大 小 的 小 块 进行 比较 ， 就 可 以 在 一 定 程度 上 提高 区 分 性 。 
这 就 是 所 谓 的 块 匹 配 。 注 意 到 在 这 个 过 程 中 ， 只 有 假设 在 不 同 图 像 间 整 
个 小 块 的 灰 度 值 不 变 ， 这 种 比较 才 有 意义 。 所 以 算法 的 假设 ， 从 像素 的 
灰 度 不 变性 ， 变 成 了 图 像 块 的 灰 度 不 变性 一 一 在 一 定 程 度 上 变 得 更 强 
Ta 

好 了 ， 现 在 我 们 取 了 p , 周围 的 小 块 ， 并 且 在 极 线 上 也 取 了 很 多 个 小 
块 。 不 妨 把 p , 周围 的 小 块 记 成 AE RW ， 把 极 线 上 的 n 个 小 块 记 成 B |i 
=1,…,n。 那 么 ， 如 何 计算 小 块 与 小 块 间 的 差异 呢 ? 有 若干 种 不 同 的 计算 
万 法 : 

1.SAD (SumofAbsoluteDi ff erence) 。 顾 名 思 义 ， 即 取 两 个 小 块 的 差 
的 绝对 值 之 和 : 


S(A, B)san = >》 |A(i, j) — B(i, j)l- (13.1) 
i,j 
2.SSD。 这 里 的 SSD 并 不 是 指 大 家 熟悉 的 固态 硬盘 ， 而 是 Sum of 
Squared Distance (平方 和 ) 的 意思 : 


S(A, B)sso = X (Ali, j) — Bi, 9)’ (13.2) 


3.NCC (Normalized Cross Correlation， 归 一 化 互相 关 ) 。 这 种 方式 比 
前 两 种 要 复杂 一 些 ， 它 计算 的 是 两 个 小 块 的 相关 性 : 
> Ali, J) BE, j) 


S(A, B)ncc = 2 z = 
VAG, I) >, B(i, j) 


(13.3) 


tJ 


请 注意 ， 由 于 这 里 用 的 是 相关 性 ， 所 以 相关 性 接近 0 表示 两 幅 图 像 不 
相似 ， 而 接近 1 才 表 示 相 似 。 前 面 两 种 距离 则 是 反 过 来 的 ， 接 近 0 表 示 相 
似 ， 而 大 的 数值 表示 不 相似 。 

和 我 们 遇 到 过 的 许多 情形 一 样 ， 这 些 计算 方式 往往 存在 一 个 精度 - 效 
率 之 间 的 矛盾 。 精 度 好 的 方法 往往 需要 复杂 的 计算 ， 而 简单 的 快速 算法 
又 往往 效果 不 佳 。 这 需要 我 们 在 实际 工程 中 进行 取舍 。 另 外 ， 除 了 这 些 


简单 版 本 之 外 ， 我 们 可 以 先 把 每 个 小 块 的 均值 去 掉 ， 称 为 去 均值 的 
SSD、 去 均值 的 NCC， 等 等 。 去 掉 均 值 之 后 ， 我 们 允许 像 “ 小 块 B kba 整 
体 上 亮 一 些 ,， 但 仍然 很 相似 ”这 样 的 情况 外 ， 因 此 比 之 前 的 更 加 可 靠 一 
些 。 如 果 读 者 对 更 多 的 块 匹 配 度量 方法 感 兴趣 ， 建 议 阅读 文献 [110,111] 作 
为 补充 材料 。 

现在 ， 我 们 在 极 线 上 计算 了 A 与 每 一 个 B, 的 相似 性 度量 。 为 了 方便 禾 
述 ， 假 设 我 们 用 了 NCC， 那 么 ， 我 们 将 得 到 一 个 沿 着 极 线 的 NCC 分 布 。 
这 个 分 布 的 形状 严重 取决 于 图 像 本 身 的 样子 ， 如 图 13-3 所 示 。 在 搜索 距离 
较 长 的 情况 下 ， 我 们 通常 会 得 到 一 个 非 凸 阔 数 : 这 个 分 布 存在 着 许多 峰 
值 ， 然 而 真实 的 对 应 点 必定 只 有 一 个 。 在 这 种 情况 下 ， 我 们 会 倾向 于 使 
用 概率 分 布 来 描述 深度 值 ， 而 非 用 某 个 单一 的 数值 来 描述 深度 。 于 是 ， 
我 们 的 问题 就 转 到 了 在 不 断 对 不 同 图 像 进行 极 线 搜索 时 ， 我 们 估计 的 深 
度 分 布 将 发 生 怎 样 的 变化 一 一 这 就 是 所 谓 的 深度 滤波 器 。 
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图 13-3 ”匹配 得 分 沿 距离 的 分 布 ， 图 像 来 自 文献 [112]。 


13.2.3 ”高 斯 分 布 的 深度 滤波 器 


对 像素 点 深度 的 估计 ， 本 身 亦 可 建 模 为 一 个 状态 估计 间 题 ， 于 是 就 
目 然 存在 滤波 器 与 非 线 性 优化 两 种 求解 思路 。 里 然 非 线性 优化 效果 较 


好 ， 但 是 在 SLAM 这 种 实时 性 要 求 较 强 的 场合 ， 考 虑 到 前 端 已 经 占据 了 不 
少 的 计算 量 ， 建 图 方面 则 通常 采用 计算 量 较 少 的 滤波 器 方式 了 。 这 也 是 
本 节 讨 论 深 度 滤波 器 的 目的 。 


对 深度 的 分 布 假设 存在 着 若干 种 不 同 的 做 法 。 首 先 ， 在 比较 简单 的 
假设 条 件 下 ， 我 们 可 以 假设 深度 值 服从 高 斯 分 布 ， 得 到 一 种 类 卡尔 曼 式 
的 方法 (我们 稍 后 会 看 到 ) 。 而 另 一 方面 ， 在 [112,56] 等 文献 中 ， 亦 采用 
了 均匀 -高 斯 混合 分 布 的 假设 ,推导 了 另 一 种 形式 更 为 复杂 的 深度 滤波 
器 。 本 着 简单 易 用 的 原则 ， 我 们 先 来 介绍 并 演示 高 斯 分 布 假设 下 的 深度 
滤 站 器， 然后 把 均匀 -高 斯 混合 分 布 的 滤波 器 作为 习题 。 


设 某 个 像素 点 的 深度 d ARM: 
P(d) = N(u, 07). (13.4) 


而 每 当 新 的 数据 到 来 ， 我 们 都 会 观测 到 它 的 深度 。 同 样 ， 假 设 这 次 
观测 亦 是 一 个 高 斯 分 布 : 
P(dobs) = N (Mobs; Oana): (13.5) 
于 是 ， 我 们 的 问题 是 ， 如 何 使 用 观测 的 信息 更 新 原先 d 的 分 布 。 这 正 
是 一 个 信息 融合 问题 。 根 据 附 录 A， 我 们 明白 两 个 高 斯 分 布 的 乘积 依然 是 
一 个 高 斯 分 布 。 设 融合 后 的 d 的 分 布 为 wa ct&J， 那 么 根据 高 斯 分 布 的 乘 
积 ， 有 : 


2 2 2 
O + oO” UL OO 
obs Hobs = obs_ (13.6) 


Hfuse = 02 十 T2, 

由 于 我 们 仅 有 观测 方程 而 没有 运动 方程 ， 所 以 这 里 深度 仅 用 到 了 信 
息 融 合 部 分 ， 而 无 须 像 完整 的 卡尔 曼 那 样 进行 预测 和 更 新 。 可 以 看 到 融 
合 的 方程 确实 比较 浅显 易 懂 ， 不 过 问题 仍然 存在 : 如 何 确定 我 们 观测 到 
深度 的 分 布 呢 ? 即 ， 如 何 计 算 x o0 os 呢 ? 


FFU ors ,0 ws ， 亦 存在 一 些 不 同 的 处 理 方式 。 例 如 ， 文 献 [59] 考 虑 了 


几何 不 确定 性 和 光度 不 确定 性 二 者 之 和 ， 而 [112] 则 仅 考 虑 了 几何 不 确定 
性 。 我 们 暂时 只 考虑 由 几何 关系 读 来 的 不 确定 性 。 现 在 ， 假 设 我 们 通过 
极 线 搜索 和 块 匹 配 确 定 了 参考 帧 某 个 像素 在 当前 帧 的 投影 位 置 。 那 么 ， 
这 个 位 置 对 深度 的 不 确定 性 有 多 大 呢 ? 


以 图 13-4 为 例 。 考 虑 某 次 极 线 搜索 ， 我 们 找到 了 p , 对 应 的 p , mr, M 
而 观测 到 了 p , 的 深度 值 ， 认 为 p , 对 应 的 三 维 点 为 P 。 从 而 ， 可 记 O , P 为 
pP，0O10 ,为 相机 的 平移 : ，O ,P 记 为 a 。 并 且 ， 把 这 个 三 角形 的 下 面 两 
个 角 记 作 %wp8 。 现 在 ， 考 虑 极 线 1 ,上 和 存在 着 一 个 像素 大 小 的 误差 ， 使 得 6 
角 变 成 了 B ， 而 p 也 变 成 了 p ， 并 记 上 面 那 个 角 为 y 。 我 们 要 问 的 是 ， 这 
一 个 像素 的 误差 会 导致 p 与 p 产生 多 大 的 差距 呢 ? 
， 这 是 一 个 典型 的 几何 问题 。 我 们 来 列 写 这 些 量 之 问 的 几何 关系 。 显 
a=p-t 
a = arccos (p, t) (13.7) 


b = arccos (a, —t) . 


图 13-4 不 确定 性 分 析 。 


对 p , 扰动 一 个 像素 ， 将 使 得 6 产生 一 个 变化 量 68 ， 由 于 相机 焦距 为 f 
， 于 是 : 


00 = arctan 3 (13.8) 


f 
所 以 : 
(13.9) 
y=r—-a-— p. 
于 是 ， 由 正弦 定理 ，p' 的 大 小 可 以 求 得 : 
Ip" = Is (13.10) 
sin y 


由 此 ， 我 们 确定 了 由 单个 像素 的 不 确定 引起 的 深度 不 确定 性 。 如 果 
认为 极 线 搜索 的 块 匹 配 仅 有 一 个 像素 的 误差 ， 那 么 就 可 以 设 : 


Tos = ||| — lp. (13.11) 
当然 ， 如 果 极 线 搜索 的 不 确定 性 大 于 一 个 像素 ， 我 们 亦 可 按照 此 推 
导 来 放大 这 个 不 确定 性 。 接 下 来 的 深度 数据 融合 ， 已 经 在 前 面 介 绍 过 
了 。 在 实际 工程 中 ， 当 不 确定 性 小 于 一 定 阅 值 之 后 ， 就 可 以 认为 深度 数 
据 已 经 收敛 了 。 


综 上 所 述 ， 我 们 给 出 了 估计 稠密 深度 的 一 个 完整 的 过 程 : 

1. 假 设 所 有 像素 的 深度 满足 某 个 初始 的 高 斯 分 布 。 

2. 当 新 数据 产生 时 ， 通 过 极 线 搜索 和 块 匹配 确定 投影 点 位 置 。 
3. 根 据 几 何 关 系 计 算 三 角 化 后 的 深 度 及 不 确定 性 。 


4. 将 当前 观测 融合 进 上 一 次 的 估计 中 。 若 收 化 则 停止 计算 ， 否 则 返回 
第 2 步 。 


这 些 步 又 组 成 了 一 套 可 行 的 深度 估计 方式 。 它 的 实际 结果 如 何 ， 我 
们 将 在 实践 部 分 进行 演示 。 


13.3 LHR: 单 目 稠密 重建 


本 节 的 示例 程序 将 使 用 REMODEM31%9 的 测试 数据 集 。 它 提供 了 一 架 
无 人 机 采集 的 单 目 俯 视图 像 ， 共 有 200 张 ， 同 时 提供 了 每 张 图像 的 真实 位 
资 。 下 面 我 们 来 考虑 ， 在 这 些 数 据 的 基础 上 估算 第 一 帧 图 像 每 个 像素 对 
应 的 深度 值 ， 即 进行 单 目 稠密 重建 。 


首先 ， 请 读者 从 http://rpg.ifi.uzh.ch/datasets/remode_test_data.zip 处 下 
载 示例 程序 所 用 的 数据 。 你 可 以 使 用 网 页 浏览 器 或 下 载 工 具 进 行 下 载 。 
解压 后 ， 将 在 test_data/Images 中 发 现 从 0 至 200 的 所 有 图 像 ， 并 在 test_data 
目录 下 看 到 一 个 文本 文件 ， 它 记录 了 每 幅 图 像 对 应 的 位 姿 : 


scene_000.png 1.086410 4.766730 -1.449960 0.789455 0.051299 -0.000779 0.611661 
scene_001.png 1.086390 4.766370 -1.449530 0.789180 0.051881 -0.001131 0.611966 
scene_002.png 1.086120 4.765520 -1.449090 0.788982 0.052159 -0.000735 0.612198 


N 


图 13-5 展 示 了 若干 时 刻 的 图 像 。 可 以 看 到 场景 主要 由 地 面 、 桌 子 及 桌 
子 上 的 杂 物 组 成 。 如 果 深 度 估 计 大 臻 正确， 那么 我 们 至 少 可 以 看 出 桌子 
与 地 面 的 深度 值 不 同 之 处 。 下 面 ， 我 们 按照 之 前 的 讲解 书写 稠密 深度 估 
计 程 序 。 为 了 方便 理解 ， 程 序 书写 成 了 C 语 言 风格 ， 放 在 单个 文件 中 。 本 
程序 稍微 有 点 长 ， 在 书 中 重点 讲解 几 个 重要 函数 ， 其 余 内 容 请 读者 对 照 
GitHub 的 源码 进行 阅读 。 


内 slambook/ch13/dense_monocular/dense_mapping.cpp (片段 ) 


#include <iostream> 


2 |#include <vector> 
3 |#include <fstream> 
4 |using namespace std; 


5 |#include <boost/timer.hpp> 


7 |// for sophus 
8 |#include <sophus/se3.h> 


t=0 t=50 t=100 


t=150 


图 13-5 ”数据 集 图 示 。 


using Sophus: :SE3; 


// for eigen 
#include <Eigen/Core> 
#include <Eigen/Geometry> 


using namespace Eigen; 


#include <opencv2/core/core.hpp> 
#include <opencv2/highgui/highgui .hpp> 
#include <opencv2/imgproc/imgproc.hpp> 


using namespace cv; 


TAA AAA AR A AAAI IAA AAA AA. 
* 本 程序 演示 了 单 目 相机 在 已 知 轨 迹 下 的 稠密 深度 估计 
* 使 用 极 线 搜索 + NCC 匹配 的 方式 ,与 本 书 13.2 节 对 应 
* 请 注意 ， 本 程序 并 不 完美 ， 你 完全 可 以 改进 它 。 
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LO SS eS 
// parameters 

const int boarder = 20; // 边缘 宽度 

const int width = 640; // 宽度 

const int height = 480; // 高 度 

const double fx = 481.2f; // 相机 内 参 

const double fy = -480.0f; 


const double cx = 319.5f; 
const double cy = 239.5f; 


const int ncc_window_size = 2; 


const int 


ncc_area = (2*ncc_ 


const double min_cov = 0.1; 


const double max_cov = 10; 


// EB BHR 
// 从 REMODE 数据 集 读 取 数据 
bool readDatasetFiles( 


const 


string& path, 


// KRABI 
// REIZ: 最 大 方差 


vector<string>& color_image_files, 


vector<SE3>& poses 


Xs 


AMV 根据 新 的 图 像 更 新 深度 估计 
bool update( 


const 
const 


const 


Mat& ref, 
Mat& curr, 


SE3& T_C_R, 


Mat& depth, 
Mat& depth_cov 


); 
// 极 线 搜索 
bool epipolarSearch( 
const Mat& ref, 
const Mat& curr, 
const SE3& T_C_R, 
const Vector2d& pt_ref, 


const 


const 


double& depth_mu, 
double& depth_cov, 


Vector2d& pt_curr 


X; 


// 更 新 深度 滤波 器 
bool updateDepthFilter( 


const 
const 


const 


Vector2d& pt_ref, 
Vector2d& pt_curr, 
SE3& T_C_R, 


Mat& depth, 
Mat& depth_cov 


// NCC 取 的 窗口 半 宽 度 
window_size+1)*(2*ncc_window_size+1); // NCC 窗 口 面积 


80 


81 


// 计算 NCC 评分 


double NCC( const Mat& ref, const Mat& curr, const Vector2d& pt_ref, const Vector2d 


& pt_curr ); 


// 双 线 性 灰 度 插值 


inline double getBilinearInterpolatedValue( const Mat& img, const Vector2d& pt ) { 
uchar* d = & img.data[ int(pt(1,0))*img.steptint(pt(0,0)) ]; 


double xx = pt(0,0) - floor(pt(0,0)); 

double yy = pt(1,0) - floor(pt(1,0)); 

return (( 1-xx ) * ( 1-yy ) * double(d[0]) + 
xx* ( 1-yy ) * double(d[1]) + 
( 1-xx ) *yy* double(d[img.step]) + 
xx*yy*double(d[img.step+i]))/255.0; 


J/ 一 些小 工具 
// 显示 估计 的 深度 图 
bool plotDepth( const Mat& depth ); 


// 像素 到 相机 坐标 系 
inline Vector3d px2cam ( const Vector2d px ) { 
return Vector3d ( 
(px(0,0) = cx)./fx, 
(px(1,0) - cy)/fy, 
1 
); 


// 相机 坐标 系 到 像素 
inline Vector2d cam2px ( const Vector3d p_cam ) { 
return Vector2d ( 
p_cam(0,0)*fx/p_cam(2,0) + cx, 
p_cam(1,0)*fy/p_cam(2,0) + cy 
)3 


// 检测 一 个 点 是 否 在 图 像 边框 内 
inline bool inside( const Vector2d& pt ) { 
return pt(0,0) >= boarder && pt(1,0)>=boarder 


&& pt(0,0)+boarder<width && pt(1,0)+boarder<=height ; 


int main( int argc, char** argv ) 


{ 


if ( argc != 2 ) 
{ 
cout<<"Usage: dense_mapping path_to_test_dataset"<<endl; 


return -1; 


// 从 数据 集 读 取 数据 
vector<string> color_image_files; 


vector<SE3> poses_TWC; 


bool ret = readDatasetFiles( argv[i], color_image_files, poses_TWC ); 


if ( ret==false ) 

{ 
cout<<"Reading image files failed!"<<endl; 
return -1; 

} 


cout<<"read total "<<color_image_files.size()<<" files."<<endl; 


// 第 一 张 图 

Mat ref = imread( color_image_files[0], O ); // gray-scale image 
SE3 pose_ref_TWC = poses_TWC[0]; 

double init_depth = 3.0; // 深度 初始 值 

double init_cov2 = 3.0; // 方差 初始 值 

Mat depth( height, width, CV_64F, init_depth ); J/ 深度 图 

Mat depth_cov( height, width, CV_64F, init_cov2 ); // 深度 图 方差 


for ( int index=1; index<color_image_files.size(); index++ ) 
{ 

cout<<"*** loop "<<index<<" *#**"<<end1; 

Mat curr = imread( color_image_files[index], 0 ); 

if (curr.data == nullptr) continue; 

SE3 pose_curr_TWC = poses_TWC[index] ; 

SE3 pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC; 

// 坐标 转换 关系 : TCW* TWR=T_CR 

update( ref, curr, pose_T_C_R, depth, depth_cov ); 

plotDepth( depth ); 

imshow("image", curr) ; 


waitKey (1) ; 


return 0; 


// 对 整个 深度 图 进行 更 新 
bool update(const Mat& ref, const Mat& curr, const SE3& T_C_R, Mat& 


depth, Mat& 


depth_cov ) 
{ 


#pragma omp parallel for 


for ( int x=boarder; x<width-boarder; x++ ) 


#pragma omp parallel for 


for ( int y=boarder; y<height-boarder; y++ ) 


{ 
} 
} 
// 极 线 搜索 


// 遍历 每 个 像素 

if ( depth_cov.ptr<double>(y) [x] < min_cov 
|| depth_cov.ptr<double>(y) [x] > max_cov ) // 深度 已 收敛 或 发 散 
continue; 

// 在 极 线 上 搜索 (x,y) 的 匹配 

Vector2d pt_curr; 

bool ret = epipolarSearch ( 
ref, 
curr, 
T_C_R, 
Vector2d(x,y), 
depth. ptr<double>(y) [x], 
sqrt (depth_cov.ptr<double>(y) [x]), 
pt_curr 


3 


if ( ret == false ) // 匹配 失败 


continue; 


// 取消 该 注释 以 显示 匹配 
// showEpipolarMatch( ref, curr, Vector2d(z,y), pt_curr ); 


// 匹配 成 功 ， 更 新 深度 图 
updateDepthFilter( Vector2d(x,y), pt_curr, T_C_R, depth, depth_cov ); 


// FEA 13.2, 13.3 两 节 
bool epipolarSearch( 


const Mat& ref, const Mat& curr, 
const SE3& T_C_R, const Vector2d& pt_ref, 


const double& depth_mu, const double& depth_cov, 


Vector2d& pt_curr 


Vector3d f_ref = px2cam( pt_ref ); 


f_ref.normalize(); 


Vector3d P_ref = f_ref*depth_mu; // 参考 帧 的 已 向量 


Vector2d px_mean_curr = cam2px( T_C_R*P_ref ); // 按 深度 均值 投影 的 像素 
double d_min = depth_mu-3*depth_cov, d_max = depth_mu+3*depth_cov; 
if ( d_min<0.1 ) d_min = 0.1; 


Vector2d px_min_curr = cam2px( T_C_R*(f_ref*d_min) );  // 按 最 小 深度 投影 的 像素 
Vector2d px_max_curr = cam2px( T_C_R*(f_ref*d_max) ); // 按 最 大 深度 投影 的 像素 


Vector2d epipolar_line = px_max_curr - px_min_curr; // MA (线段 形式 ) 
Vector2d epipolar_direction = epipolar_line; // 极 线 方向 
epipolar_direction.normalize() ; 

double half_length = 0.5*epipolar_line.norm(); // 极 线 线段 的 半 长 度 

if ( half_length>100 ) half_length = 100; // 我 们 不 希望 搜索 太 多 东西 


// 取消 此 名 注释 以 显示 极 线 (线段) 


// showEpipolarLine( ref, curr, pt_ref, px_min_curr, px_maz_curr ); 


// 在 极 线 上 搜索 ， 以 深度 均值 点 为 中 心 ， 左 右 各 取 半 长 度 
double best_ncc = -1.0; 
Vector2d best_px_curr; 
for ( double 1=-half_length; 1<=half_length; 1+=0.7 ) // l+t=sqrt(2) 
{ 
Vector2d px_curr = px_mean_curr + l*epipolar_direction; // 待 匹配 点 
if ( !inside(px_curr) ) 
continue; 
// 计算 待 匹 配点 与 参考 帧 的 WCC 
double ncc = NCC( ref, curr, pt_ref, px_curr ); 


if ( ncc>best_ncc ) 


{ 
best_ncc = ncc; 
best_px_curr = px_curr; 
} 
} 
if ( best_ncc < 0.85f ) // 只 相信 NCC 很 高 的 匹配 


return false; 
pt_curr = best_px_curr; 


return true; 


double NCC ( 


const Mat& ref, const Mat& curr, 
const Vector2d& pt_ref, const Vector2d& pt_curr 


// 零 均 值 - 归 一 化 互相 关 


// 先 算 均 值 

double mean_ref = 0, mean_curr = 0; 

vector<double> values_ref, values_curr; // 参考 帧 和 当前 帧 的 均值 
for ( int x=-ncc_window_size; x<=ncc_window_size; x++ ) 


for ( int y=-ncc_window_size; y<=ncc_window_size; y++ ) 


{ 
double value_ref = double(ref.ptr<uchar>( int(y+pt_ref(1,0)) )[ int(x+ 
pt_ref(0,0)) ])/255.0; 
mean_ref += value_ref; 
double value_curr = getBilinearInterpolatedValue( curr, pt_curr+ 
Vector2d(x,y) ); 
mean_curr += value_curr; 
values_ref.push_back(value_ref) ; 
values_curr.push_back(value_curr) ; 
} 


mean_ref /= ncc_area; 


mean_curr /= ncc_area; 


// 计算 Zero mean NCC 
double numerator = 0, demoniatori = 0, demoniator2 = 0; 


for ( int i=0; i<values_ref.size(); i++ ) 


{ 
double n = (values_ref[i]l-mean_ref) * (values_curr[i]-mean_curr) ; 
numerator += n; 
demoniatori += (values_ref[i]-mean_ref)*(values_ref[i]-mean_ref) ; 
demoniator2 += (values_curr[i]-mean_curr)*(values_curr[i]-mean_curr) ; 

} 

return numerator / sqrt( demoniatori*demoniator2+ie-10 );  // 防止 分 母 出 现 零 


bool updateDepthFilter( 


const Vector2d& pt_ref, 
const Vector2d& pt_curr, 
const SE3& T_C_R, 

Mat& depth, 

Mat& depth_cov 


// 用 三 角 化 计算 深度 
SE3 T_R_C = T_C_R.inverse(); 
Vector3d f_ref = px2cam( pt_ref ); 


f_ref.normalize(); 


Vector3d f_curr = px2cam( pt_curr ); 


f_curr.normalize(); 


// 方程 参照 本 书 第 7 讲 三 角 化 一 节 
Vector3d t = T_R_C.translation(); 
Vector3d f2 = T_R_C.rotation_matrix() * f_curr; 
Vector2d b = Vector2d ( t.dot ( f_ref ), t.dot ( f2 ) ); 
double A[4]; 
A[0] = f_ref.dot ( f_ref ); 
A[2] = f_ref.dot ( f2 ); 
A[1] -A [2]; 
A [3] - £2:dot ( £2 ); 
double d = A[0]*A[3]-A[1] *A[2]; 
Vector2d lambdavec = 
Vector2d ( A[3] * b ( 0,0 ) - Afi] * b (1,0), 
-A[2] * b ( 0,0 ) + ALOJ * b ( 1,0 )) Já; 
Vector3d xm = lambdavec ( 0,0 ) * f_ref; 
Vector3d xn = t + lambdavec ( 1,0 ) * £2; 
Vector3d d_esti = ( xmtxn ) / 2.0; // 三 角 化 算得 的 深度 向 量 
double depth_estimation = d_esti.norm(); // 深度 值 


i] 


// 计算 不 确定 性 (以 一 个 像素 为 误差 ) 

Vector3d p = f_ref*depth_estimation; 

Vector3d a = p - t; 

double t_norm = t.norm(); 

double a_norm = a.norm(); 

double alpha = acos( f_ref.dot(t)/t_norm ); 
double beta = acos( -a.dot(t)/(a_norm*t_norm)); 
double beta_prime = beta + atan(1/fx); 

double gamma = M_PI - alpha - beta_prime; 
double p_prime = t_norm * sin(beta_prime) / sin(gamma); 
double d_cov = p_prime - depth_estimation; 


double d_cov2 = d_cov*d_cov; 
// 高 斯 融合 
double mu = depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ]; 


double sigma2 = depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ]; 


double mu_fuse = (d_cov2*mut+sigma2*depth_estimation) / ( sigma2+d_cov2); 
double sigma_fuse2 = ( sigma2 * d_cov2 ) / ( sigma2 + d_cov2 ); 


depth. ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = mu_fuse; 
depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = sigma_fuse2; 


return true; 


46 |} 


347 
348 | // 其 他 次 要 的 函数 略 


如 果 读 者 理解 了 上 一 节 内 容 ， 相 信 读 懂 此 处 源 代码 亦 不 是 难事 。 尽 
管 如 此 ， 我 们 对 几 个 关键 国 数 稍 做 说 明 : 

1.main 闵 数 非常 简单 。 它 只 负责 从 数据 集中 读 取 图 像 ， 然 后 交 给 
update 遂 数 ， 对 深度 图 进行 更 新 。 

2.update 闵 数 中 ， 我 们 遍历 了 参考 帧 的 每 个 像素 ， 先 在 当前 帧 中 寻找 
极 线 匹 配 ， 若 能 匹配 上 ， 则 利用 极 线 匹 配 的 结果 更 新 深度 图 的 估计 。 


3. 极 线 搜索 原理 大 致 和 上 一 节 介 绍 的 相同 ， 但 实现 上 添加 了 一 些 细 
节 : 因为 假设 深度 值 服从 高 斯 分 布 ， 我 们 就 以 均值 为 中 心 ， 左 右 各 取 + 30 
作为 半径 ， 然 后 在 当前 帧 中 寻找 极 线 的 投影 。 然 后 ， 遍 历 此 极 线 上 的 像 
R (FKE vi 的 近似 值 0.7) ， 寻 找 NCC 最 高 的 点 作为 匹配 点 。 如 果 最 
高 的 NCC 也 低 于 阅 值 《这 里 取 0.85) ， 则 认为 匹配 失败 。 


4.NCC 的 计算 使 用 了 去 均值 化 后 的 做 法 ， 即 对 于 图 像 块 A,B ， 取 : 


> (AG j) — AG, j)) (BGI) — BU 5) 


(A, B) = —2 - - - =. (13.12) 
/3 (Ali, i) — Ai i) E (BUA) — BY. 3)) 


NCC, 


5. 三 角 化 的 计算 方式 与 7.5 节 一 致 ， 不 确定 性 的 计算 与 高 斯 融合 方法 
和 上 一 节 一 致 。 
虽然 程序 有 些 长 ， 相 信 读 者 根据 上 面 的 提示 能 读 懂 。 下 面 我 们 来 看 


一 、 人 一 /一 全 


实验 结果 
编译 此 程序 后 ， 以 数据 集 目录 作为 参数 运行 之 : 


1 |$ build/dense_mapping ~/dataset/test_data 
2 |read total 202 files. 
3 2 KOK loop 1 *** 


4 | KK loop 2 *** 


fz RMA SLR, NERT AERAR ERRIRE 
图 。 关 于 深度 图 ， 我 们 显示 的 是 深 度 值 乘 以 0.4 后 的 结果 一 一 也 就 是 纯 日 
点 (数值 为 1.0) 的 深度 约 2.5 米 ， 颜 色 越 深 表 示 深 度 值 越 小 ， 也 就 是 物体 
离 我 们 越 近 。 如 果实 际 运行 了 程序 ， 应 该 会 发 现 深 度 估 计 是 一 个 动态 的 
过 程 一 一 从 一 个 不 怎么 确定 的 初始 值 逐渐 收 全 到 稳定 值 的 过 程 。 我 们 的 
初始 值 使 用 了 均值 和 方差 均 为 3.0 的 分 布 。 当 然 你 也 可 以 修改 初始 分 布 ， 
看 看 对 结果 会 产生 怎样 的 影响 。 


从 图 13-6 可 以 发 现 ， 当 达 代 次 数 超过 一 定 值 之 后 ， 深 度 图 趋 于 稳定 ， 
不 再 对 新 的 数据 产生 改变 。 观 察 稳 定之 后 的 深度 图 ， 我 们 发 现 大 致 可 以 
看 出 地 板 和 桌子 的 区 别 ， 而 桌 上 的 物体 深度 则 接近 于 桌子 。 整 个 估计 大 
部 分 是 正确 的 ， 但 也 存在 着 大 量 错误 估计 。 它 们 表现 为 深度 图 中 与 周围 
数据 不 一 致 的 地 方 ， 为 过 大 或 过 小 的 估计 。 此 外 ， 位 于 边缘 处 的 地 方 ， 
由 于 运动 过 程 中 看 到 的 次 数 较 少 ， 所 以 亦 没 有 得 到 正确 的 估计 。 绽 上 所 
述 ， 我 们 认为 这 个 深度 图 的 大 部 分 是 正确 的 ， 但 没有 达到 预想 的 效果 。 
我 们 将 在 下 一 节 分 析 这 些 情况 的 出 现 原因 ， 并 讨论 有 哪些 可 以 改进 的 地 
方 。 


— 


E 
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序 运行 时 截图 。 两 图 分 别 是 迭代 10 次 和 30 次 的 结果 。 


图 13-6 ”演示 程 


13.4 ”实验 分 析 与 讨论 


上 一 节 我 们 演示 了 移动 单 目 相机 的 稠密 建 图 ， 估 计 了 参考 帧 的 每 个 
像素 深度 。 我 们 的 代码 是 相对 简单 直接 的 ， 没 有 使 用 许多 的 技巧 
(trick) ， 因 此 出 现 了 实际 工程 中 常见 的 情形 一 一 简单 的 往往 并 不 是 最 有 
效 的 。 

由 于 真实 数据 的 复杂 性 ， 能 够 在 实际 环境 下 工作 的 程序 ， 往 往 需 
周密 的 考虑 和 大 量 的 工程 技巧 ， 这 使 得 每 种 实际 可 行 的 代码 都 极其 复杂 
一 一 它们 很 难 向 初学 者 解释 清楚 ， 所 以 我 们 只 好 使 用 不 那么 有 效 ， 但 相 
对 易 读 易 写 的 实现 方式 。 我 们 当然 可 以 提出 若干 种 对 演示 程序 加 以 改进 
的 意见 ， 不 过 这 里 并 不 打算 把 已 经 改 好 的 (非常 复杂 的 ) 程序 直接 呈现 
给 读者 。 


下 面 我 们 对 上 一 实验 的 结果 进行 初步 分 析 。 我 们 将 从 计算 机 视觉 
和 滤波 器 两 个 角度 来 分 析 演 示 实 验 的 结果 。 


13.4.1 ”像素 梯度 的 问题 


对 深度 图 像 进行 观察 ， 我 们 会 发 现 一 件 明 显 的 事实 。 块 匹配 的 正确 
与 否 ， 依 赖 于 图 像 块 是 否 具有 区 分 度 。 显 然 ， 如 果 图 像 块 仅 是 一 片 黑 或 
者 一 片 白 ， 缺 少 有 效 的 信息 ， 那 么 在 NCC 计 算 中 我 们 就 很 可 能 错误 地 将 
它 与 周围 的 某 块 像素 给 匹配 起 来 。 请 读者 观察 壮 示 程序 中 的 打印 机 表 
面 。 由 于 它 是 均匀 的 白色 ， 非 常 容易 引起 误 匹 配 ， 因 此 打印 机 表面 的 深 
度 信 息 多 半 是 不 正确 的 一 一 示例 程序 的 空间 表面 出 现 了 明显 不 该 有 的 条 
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这 里 牵涉 到 了 一 个 问题 ， 该 问题 在 直接 法 中 我 们 已 经 见 过 一 次 。 在 
进行 块 匹 配 (和 NCC 的 计算 ) 时， 我 们 必须 假设 小 块 不 变 ， 然 后 将 该 小 
块 与 其 他 小 块 进行 对 比 。 这 时 ， 有 了 明显 梯度 的 小 块 将 具有 良好 的 区 分 
度 ， 不 易 引 起 误 匹 配 。 对 于 梯度 不 明显 的 像素 ， 由 于 在 块 匹 配 时 没有 区 
分 性 ， 所 以 我 们 将 难以 有 效 地 估计 其 深度 。 反 之 ， 像 素 梯 度 比较 明显 的 
地 方 ， 我 们 得 到 的 深度 信息 也 相对 准确 ， 例 如 桌面 上 的 杂志 、 电 话 等 具 
有 明显 纹理 的 物体 。 因 此 ， 演 示 程 序 反映 了 立体 视觉 中 一 个 非常 常见 的 
问题 : 对 物体 纹理 的 依赖 性 。 该 问题 在 双 目 视觉 中 也 极其 常见 ， 体 现 了 
立体 视觉 的 重建 质量 十 分 依赖 于 环境 纹理 。 


我 们 的 演示 程序 刻意 使 用 了 纹理 较 好 的 环境 ， 例 如 ， 像 棋盘 格 一 般 
的 地 板 ， 审 有 木 纹 的 桌面 ， 等 等 ， 因 此 能 得 到 一 个 看 似 不 错 的 结果 。 然 
而 在 实际 中 ， 像 墙 面 、 光 滑 物体 表面 等 亮度 均匀 的 地 方 将 经 常 出 现 ， 影 
响 我 们 对 它 的 深度 估计 。 从 某 种 角度 来 说 ， 该 问题 是 无 法 在 现 有 的 算法 
流程 上 加 以 改进 并 解决 的 一 一 如 果 我 们 依然 只 关心 某 个 像素 周围 的 邻 域 
(小 块 ) 的 话 。 


进一步 讨论 像素 梯度 问题 ， 我 们 还 会 发 现 像素 梯度 和 极 线 之 间 的 联 
系 。 文 章 [59] 详 细 讨论 过 它们 的 关系 ， 不 过 在 我 们 的 演示 程序 里 也 有 直观 
的 体现 。 

以 图 13-7 为 例 ， 我 们 举 两 种 比较 极端 的 情况 : 像素 梯度 平行 于 极 线 方 
向 ， 以 及 垂直 于 极 线 方向 。 先 来 看 垂直 的 情况 。 在 垂直 的 例子 里 ， 即 使 
小 块 有 明显 梯度 ， 但 当 我 们 沿 着 极 线 去 做 块 匹 配 时 ， 会 发 现 匹 配 程度 都 


是 一 样 的 ， 因 此 得 不 到 有 效 的 匹配 。 反 之 ， 在 平行 的 例子 里 ， 我 们 能 够 
精确 地 确定 匹配 度 最 高 点 出 现在 何 处 。 而 实际 当中 ， 梯 度 与 极 线 的 情况 
很 可 能 介 于 二 者 之 间 : 既 不 是 完全 垂直 亦 不 是 完全 平行 。 这 时 ， 我 们 
说 ， 当 像素 梯度 与 极 线 夹 角 较 大 时 ， 极 线 匹配 的 不 确定 性 大 ;而 当 夹 角 
较 小 时 ， 匹 配 的 不 确定 性 变 小 。 而 在 演示 程序 中 ， 我 们 统一 地 把 这 些 情 
况 都 当成 一 个 像素 的 误差 ， 实 际 是 不 够 精细 的 。 考 虑 到 极 线 与 像素 梯度 
的 关系 后 ， 应 该 使 用 更 精确 的 不 确定 性 模型 。 具 体 的 调整 和 改进 留 作 习 


题 。 


极 线 


梯度 平行 于 极 线 像素 梯度 垂直 于 极 线 


图 |. 


图 13-7 像素 梯度 与 极 线 之 关系 示意 图 。 


13.4.2 PRE 


从 另 一 个 角度 看 ， 我 们 不 妨 问 : 把 像素 深度 假设 成 高 斯 分 布 是否 合 
适 呢 ? 这 里 关系 到 一 个 参数 化 的 问题 (Parameterization) 。 


在 前 面 的 内 容 中 ， 我 们 经 常用 一 个 点 的 世界 坐标 x,y,z 三 个 量 来 描述 

它 ， 这 是 一 种 参数 化 形式 。 我 们 认为 x,y,z 三 个 量 都 是 随机 的 ， 它 们 服从 

(三 维 的 ) 高 斯 分 布 。 然 而 ， 本 讲 使 用 了 图 像 坐标 u,v 和 深度 值 d 来 描述 

某 个 空间 点 ( 即 稠密 建 图 ) 。 我 们 认为 uv 不 动 ， 而 d 服从 (一 维 的 ) 高 
斯 分 布 ， 这 是 另 一 种 参数 化 形式 。 那 么 我 们 要 问 : 这 两 种 参数 化 形式 有 
什么 不 同 吗 ? 我 们 是 否 也 能 假设 u,v 服从 高 斯 分 布 ， 从 而 形成 另 一 种 参数 
化 形式 呢 ? 

不 同 的 参数 化 形式 ， 实 际 都 描述 了 同一 个 量 ， 也 就 是 某 个 三 维 空间 
点 。 考 虑 到 当 我 们 在 相机 看 到 某 个 点 时 ， 它 的 图 像 坐标 u,v 是 比较 确定 的 
5 ， 而 深度 值 d 则 是 非常 不 确定 的 。 此 时 ， 若 用 世界 坐标 x,y,z 描述 这 个 
点 ， 那 么 根据 相机 当前 的 位 姿 ，x,y,z 三 个 量 之 间 可 能 存在 明显 的 相关 
性 。 反 映 在 协 方差 矩阵 中 ， 表 现 为 非 对 角 元 素 不 为 零 。 而 如 果 用 wvwd 参 
数 化 一 个 点 ， 那 么 它 的 u,v Md 至 少 是 近似 独立 的 ， 甚 至 我 们 还 能 认为 u,v 
也 是 独立 的 一 一 从 而 它 的 协 方差 矩阵 近似 为 对 角 阵 ， 更 为 简洁 。 

逆 深 度 (Inverse depth) 是 近年 来 SLAM 研 究 中 出 现 的 一 种 广泛 使 用 
的 参数 化 技巧 叫 435 。 在 演示 程序 中 ， 我 们 假设 深度 值 满 足 高 斯 分 布 : d 
~N (1,0? )。 然 而 这 样 做 合 不 合理 呢 ? 深度 真 的 近似 于 一 个 高 斯 分 布 吗 ? 
仔细 想 想 ， 深 度 的 正 态 分 布 确 实 存 在 一 些 问 题 : 

1. 我 们 实际 想 表 达 的 是 : 这 个 场景 深度 大 概 是 5~10 米 ， 可 能 有 一 些 
更 远 的 点 ， 但 近 处 肯定 不 会 小 于 相机 焦距 (或 认为 深度 不 会 小 于 0) o X 
个 分 布 并 不 是 像 高 斯 分 布 那 样 ， 形 成 一 个 对 称 的 形状 。 它 的 尾部 可 能 稍 
长 ， 而 负数 区 域 则 为 零 。 

2. 在 一 些 室外 应 用 中 ， 可 能 存在 距离 非常 远 ， 旋 至 无 穷 远 处 的 点 。 我 
们 的 初始 值 中 难以 涵盖 这 些 点 ， 并 且 用 高 斯 分 布 描述 它们 会 有 一 些 数 值 
计算 上 的 困难 。 

于 是 ， 逆 深度 应 运 而 生 。 人 们 在 仿真 中 发 现 ， 假 设 深 度 的 倒数 ， 也 
就 是 逆 深 度 ， 为 高 斯 分 布 是 比较 有 效 的 。 随 后 ， 在 实际 应 用 中 ， 逆 深 
度 也 具有 更 好 的 数值 稳定 性 ， 从 而 逐渐 成 为 一 种 通用 的 技巧 ， 存 在 于 现 
有 SLAM 方 案 中 的 标准 做 法 中 5657731 。 

把 演示 程序 从 正 深 度 改 成 逆 深 度 亦 不 复杂 。 只 要 在 前 面 出 现 深 度 的 
推导 中 ， 将 q 改 成 逆 深 度 q-! 即 可 。 我 们 亦 把 这 个 改动 留 作 习题 ， 交 给 读 
者 完成 。 


13.4.3 ”图 像 间 的 变换 


在 块 匹配 之 前 ， 做 一 次 图 像 到 图 像 间 的 变换 亦 是 一 种 常见 的 预 处 理 
方式 。 这 是 因为 ， 我 们 假设 了 图 像 小 块 在 相机 运动 时 保持 不 变 ， 而 这 个 
假设 在 相机 平移 时 〈 示 例 数据 集 基本 都 是 这 样 的 例子 ) 能 够 保持 成 立 ， 
但 当 相机 发 生 明 显 的 旋转 时 ， 就 难以 继续 保持 了 。 特 别 地 ， 当 相机 绕 光 
心 旋转 时 ， 一块 下 黑 上 白 的 图 像 可 能 会 变 成 一 个 上 黑 下 白 的 图 像 块 ， 导 
致 相关 性 直接 变 成 了 负数 (尽管 仍然 是 同样 一 个 块 ) 。 


为 了 防止 这 种 情况 的 出 现 ， 我 们 通常 需要 在 块 匹 配 之 前 ， 把 参考 帧 
与 当前 帧 之 间 的 运动 考虑 进来 。 根 据 相机 模型 ， 参 考 帧 上 的 一 个 像素 P、 


与 真实 的 三 维 点 世界 坐标 P, 有 以 下 关系 : 
dRPR = K (Raw Pw + trw). (13.13) 
类 似 地 ， 对 于 当前 帧 ， 亦 有 P,, 在 它 上 边 的 投影 ， 记 作 P。: 
do Po = K (Rew Pw + tew). (13.14) 
代入 并 消去 P, ， 即 得 两 幅 图 像 之 间 的 像素 关系 : 
doPc = dRK RewRIwK '!Ppr + Ktcw — K Row RhwKtrw. (13.15) 


当知 道 d, Pe 时 ， 可 以 计算 出 P_ 的 投影 位 置 。 此 时 ， 再 给 P, 两 个 分 
量 各 一 个 增 量 du,dv ， 就 可 以 求 得 P_ 的 增 量 du_ ,dv. 。 通 过 这 种 方式 ， 算 
出 在 局 部 范围 内 参考 帧 和 当前 帧 图 像 坐 标 变换 的 一 个 线性 关系 构成 仿 射 


变换 : 
duc duc due d 
Me | _ | du “dv i. (13.16) 
dve de we. dv 


RHAI SRE, RTAS (REE) 的 像素 进行 变 
换 后 再 进行 块 匹 配 ， 以 期 获得 对 旋转 更 好 的 效果 。 


13.4.4 ”并 行 化 : 效率 的 问题 


在 实验 当中 我 们 也 看 到 ， 稠 密 深 度 图 的 估计 非常 费时 ， 这 是 因为 我 
们 要 估计 的 点 从 原先 的 数 百 个 特征 点 一 下 子 变 成 了 几 十 万 个 像素 点 ， 即 
使 现在 主流 的 CPU 也 无 法 实时 地 计算 那样 庞大 的 数量 。 不 过 ， 该 问题 亦 
有 另 一 个 性 质 : 这 几 十 万 个 像素 点 的 深度 估计 是 彼此 无 天 的 ! 这 使 并 行 
化 有 了 用 武之 地 。 


在 示例 程序 中 ， 我 们 在 一 个 二 重 循环 里 遍历 了 所 有 像素 ， 并 逐个 对 
它们 进行 极 线 搜 索 。 当 我 们 使 用 CPU 时 ， 这 个 过 程 是 串 行进 行 的 : 必须 
是 上 一 个 像素 计算 完毕 后 ， 再 计算 下 一 个 像素 。 然 而 实际 上 ， 下 一 个 像 
素 完全 没有 必要 等 待 上 一 个 像素 的 计算 结束 ， 因 为 它们 之 间 并 没有 明显 
的 联系 ， 所 以 我 们 可 以 用 多 个 线程 ， 分 别 计算 每 个 像素 ， 然 后 将 结果 统 
一 起 来 。 理 论 上 ， 如 果 我 们 有 30 万 个 线程 ， 那 么 该 问题 的 计算 时 间 和 计 
算 一 个 像素 是 一 样 的 。 


GPU 的 并 行 计算 以 构 非 常 适 合 这 样 的 问题 ， 因 此 ， 在 单 双 和 双 目 的 
稠密 重建 中 ， 经 常 看 到 利用 GPU 进行 并 行 加 速 的 方式 。 当 然 ， 本 书 不 准 
备 涉 及 GPU 编程 ， 所 以 我 们 在 这 里 仅 指 出 利用 GPU 加 速 的 可 能 性 ， 具 体 
实践 留 给 读者 作为 验证 。 根 据 一 些 类 似 的 工作 ， 利 用 GPU 的 稠密 深度 估 
计 是 可 以 在 主流 GPU 上 实时 化 的 。 


13.4.5 “其 他 的 改进 


事实 上 ， 我 们 还 能 提出 许多 对 本 例 程 进行 改进 的 方案 ， 例 如 : 

1. 现 在 各 像素 完全 是 独立 计算 的 ， 可 能 存在 这 个 像素 深度 很 小 ， 边 上 
一 个 又 很 大 的 情况 。 我 们 可 以 假设 深度 图 中 相 邻 的 深度 变化 不 会 太 大 ， 
从 而 给 深度 估计 加 上 了 空间 正则 项 。 这 种 做 法 会 使 得 到 的 深度 图 更 加 平 
ine 

2. 我 们 没有 显 式 地 处 理 外 点 (Outlier) 的 情况 。 事 实 上 ， 由 于 遮挡 、 
光照 、 运 动 模糊 等 各 种 因素 的 影响 ， 不 可 能 对 每 个 像素 都 能 保持 成 功 匹 
配 。 而 演示 程序 的 做 法 中 ， 只 要 NCC 大 于 一 定 值 ， 就 认为 出 现 了 成 功 的 
匹配 ， 没 有 考虑 到 错误 匹配 的 情况 。 

处 理 错 误 匹 配 亦 有 若干 种 方式 。 人 例如， 文献 [112] 提 出 的 均匀 -高 斯 混 
合 分 布下 的 深度 滤 疲 器 ， 显 式 地 将 内 点 与 外 点 进行 区 别 并 进行 概率 建 


模 ， 能 够 较 好 地 处 理 外 点 数据 。 然 而 这 种 类 型 的 滤 疲 器 理论 较为 复杂 ， 
本 书 不 想 过 多 涉及 ， 读 者 可 以 阅读 原始 论文 。 


从 上 面 的 讨论 可 以 看 出 ， 存 在 着 许多 可 能 的 改进 方案 。 如 果 我 们 细 
致 地 改进 每 一 步 的 做 法 ， 最 后 是 有 希望 得 到 一 个 良好 的 稠密 建 图 的 方案 
的 。 然 而 ， 正 如 我 们 所 讨论 的 ， 有 一 些 问 题 存在 理论 上 的 困难 ， 例 如 对 
环境 纹理 的 依赖 ， 又 如 像素 梯度 与 极 线 方向 的 关联 (以 及 平行 的 情 
况 ) 。 这 些 问题 很 难 通过 调整 代码 实现 来 解决 。 所 以 ， 直 到 目前 为 止 ， 
尽管 双 目 和 移动 单 目 能 够 建立 稠密 的 地 图 ， 我 们 通常 认为 它们 过 于 依赖 
于 环境 纹理 和 光照 ， 不 够 可 靠 。 


13.5 RGB-D 和 稠密 建 图 


除了 使 用 单 目 和 双 目 进行 稠密 重建 之 外 ， 在 适用 范围 内 ，RGB-D 相 
机 是 一 种 更 好 的 选择 。 在 上 一 讲 中 详细 讨论 的 深度 估计 问题 ， 在 RGB-D 
相机 中 可 以 完全 通过 传感器 中 硬件 测量 得 到 ， 无 须 消 耗 大 量 的 计算 资源 
来 估计 。 并 且 ，RGB-D 的 结构 光 或 飞 时 原理 ， 保 证 了 深度 数据 对 纹理 的 
无 关 性 。 即 使 面 对 纯 色 的 物体 ， 只 要 它 能 够 反射 光 ， 我 们 就 能 测量 到 它 
的 深度 。 这 亦 是 RGB-D 传 感 器 的 一 大 优势 。 


利用 RGB-D 进 行 稠密 建 图 是 相对 容易 的 。 不 过 ， 根 据 地 图 形式 不 
同 ， 也 存在 着 和 若干 种 不 同 的 主流 建 图 方式 。 最 直观 、 最 简单 的 方法 ， 就 
是 根据 估算 的 相机 位 姿 ， 将 RGB-D 数 据 转化 为 点 云 (Point Cloud) ， 然 
后 进行 拼接 ， 最 后 得 到 一 个 由 离散 的 点 组 成 的 点 云 地 图 (Point Cloud 
Map) 。 在 此 基础 上 ， 如 果 我 们 对 外 观 有 进一步 的 要 求 ， 和 希望 估计 物体 的 
表面 ， 可 以 使 用 三 角 网 格 (Mesh) . MA (Surfe) 进行 建 图 。 另 一 方 
面 ， 如 果 希 望 知道 地 图 的 障碍 物 信息 并 在 地 图 上 导航 ， 亦 可 通过 体 素 
(Voxel) 建立 占据 网 格 地 图 (Occupancy Map) 。 

我 们 似乎 引入 了 很 多 新 概念 。 请 读者 不 要 着 急 ， 我 们 将 慢 慢 地 逐一 
加 以 介绍 。 对 于 部 分 适合 进行 实验 的 ， 我 们 亦 会 像 往 常 一样 ， 提 供 若 干 
个 演示 程序 。 由 于 RGB-D 建 图 牵涉 到 的 理论 知识 并 不 很 多 ， 所 以 下 面 几 
节 就 直接 以 实践 部 分 来 介绍 了 。GPU 建 图 超出 了 本 书 的 范围 ， 我 们 就 简 
单 讲 解 其 原理 ， 不 做 演示 。 


13.5.1 SH: 点 云 地 图 


首先 ， 我 们 来 讲解 最 简单 的 点 云 地 图 。 所 谓 点 云 ， 就 是 由 一 组 离散 
的 点 表示 的 地 图 。 最 基本 的 点 包含 xy,z 三 佳 坐标 ， 也 可 以 融 有 759g,b 的 彩 
色 信息 。 由 于 RGB-D 相 机 提供 了 彩色 图 和 深度 图 ， 很 容易 根据 相机 内 人 参 
来 计算 RGB-D 点 云 。 如 果 通过 时 种 手段 得 到 了 相机 的 位 姿 ， 那 么 只 要 
直接 把 点 云 进行 加 和 ， 就 可 以 获得 全 局 的 点 云 。 在 本 书 的 5.4 节 ， 曾 给 出 
了 一 个 通过 相机 内 外 参 拼接 点 云 的 例子 。 不 过 ， 那 个 例子 主要 是 为 了 让 
读者 理解 相机 的 内 外 参 ， 而 在 实际 建 图 当中 ， 我 们 还 会 对 点 云 加 一 些 滤 
站 处理， 以 获得 更 好 的 视觉 效果 。 在 本 程序 中 ， 我 们 主要 使 用 两 种 滤波 
器 : 外 点 去 除 滤 波 器 ， 以 及 降 采 样 滤波 器 。 示 例 程序 的 代码 如 下 (由 于 
部 分 代码 与 之 前 的 相同 ， 我 们 主要 看 改变 的 部 分 ) : 


kA slambook/ch13/dense_RGBD/pointcloud_mapping.cpp (片段 ) 


1 | int main( int argc, char** argv ) 

2 | 4 

3 // 图 像 读 取 部 分 略 

4 // 定义 点 云 使 用 的 格式 : 这 里 用 的 是 XYZRGB 

5 typedef pcl::PointXYZRGB PointT; 

typedef pcl::PointCloud<PointT> PointCloud; 


// 新 建 一 个 点 云 
PointCloud::Ptr pointCloud( new PointCloud ) ; 
10 for ( int i=0; i<5; i++ ) 
1 { 
12 PointCloud::Ptr current( new PointCloud ); 
13 cout<<" 转 换 图 像 中 : "<<i+1<<endl; 
14 cv::Mat color = colorImgs[i] ; 
15 cv::Mat depth = depthImgs [i]; 
16 Eigen: :Isometry3d T = poses [ij] ; 
17 for ( int v=0; v<color.rows; V++ ) 
18 for ( int u=0; u<color.cols; ut++ ) 
19 { 
20 unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度 值 


21 if ( d==0 ) continue; // 4 0 表示 没有 测量 到 


if ( d >= 7000 ) continue; // 深度 太 大 时 不 稳定 ， 去 掉 
Eigen::Vector3d point; 


point[2] = double(d)/depthScale; 
point[0] = (u-cx)*point [2]/fx; 
point[1] = (v-cy)*point [2]/fy; 


Eigen::Vector3d pointWorld = T*point; 


PointT p ; 

p-xX = pointWorld [0]; 

p-y = pointWorld[1] ; 

p-z = pointWorld[2] ; 

p.b = color.data[ v*color.steptu*color.channels() ]; 
p-g = color.data[ v*color.step+u*color.channels()+1 ]; 
p-r = color.data[ v*color.step+tu*color.channels()+2 ]; 


current->points.push_back( p ); 
} 
// depth filter and statistical removal 
PointCloud::Ptr tmp ( new PointCloud ); 
pel: :StatisticalOutlierRemoval<PointT> statistical_filter; 
statistical_filter.setMeank (50) ; 
statistical_filter.setStddevMulThresh(1.0); 
statistical_filter.setInputCloud(current) ; 
statistical_filter.filter( *tmp ); 
(*pointCloud) += *tmp; 


pointCloud->is_dense = false; 
cout<<" 4 & #4 "<<pointCloud->size()<<"4 4 ."<<end1; 


// voxel filter 

pcl::VoxelGrid<PointT> voxel_filter; 

voxel_filter.setLeafSize( 0.01, 0.01, 0.01 ); // resolution 
PointCloud::Ptr tmp ( new PointCloud ); 
voxel_filter.setInputCloud( pointCloud ); 

voxel_filter.filter( *tmp ); 

tmp->swap( *pointCloud ); 


cout<<!" 滤 波 之 后 ， 点 云 共有 "<<pointCloud->size()<<" 个 点 ."<<endl; 


pcl::io::savePCDFileBinary("map.pcd"，*pointCloud ) ; 


return 0; 


我 们 的 思路 没有 太 大 变化 ， 主 要 不 同 之 处 在 于 : 

1. 在 生成 每 帧 点 云 时 ， 去 挥 深度 值 太 大 或 无 效 的 点 。 这 主要 是 考虑 到 
Kinect 的 有 效 量程 ， 超 过 量程 之 后 的 深度 值 会 有 较 大 误差 。 

2. 利 用 统计 滤波 器 方法 去 除了 珊 立 点 。 该 滤波 器 统计 每 个 点 与 距离 它 最 
近 的 N 个 点 的 距离 值 的 分 布 ， 去 除 距离 均值 过 大 的 点 。 这 样 ， 我 们 保留 
了 那些 “ 粘 在 一 起 ”的 点 ， 去 掉 了 孤立 的 噪声 点 。 


3. 最 后 ， 利 用 体 素 滤波 器 (Voxel Filter) 进行 降 采样 。 由 于 多 个 视角 
存在 视野 重 便 ， 在 重合 区 域 会 存在 大 量 的 位 置 十 分 相近 的 点 。 这 会 日 日 
占用 许多 内 存 空 间 。 体 素 滤 波 保证 了 在 某 个 一 定 大 小 的 立方 体 (或 称 体 
R) 内 仅 有 一 个 点 ， 相 当 于 对 三 维 空间 进行 了 降 采 样 ， 从 而 可 节省 很 多 
存储 空间 。 

图 13-8 显 示 了 滤波 前 后 的 对 比 图 。 左 侧 是 第 5 讲 程 序 生 成 的 点 云 地 
图 ， 而 右 侧 是 经 过 滤 疲 后 的 点 云 地 图 。 观 察 日 色 框 中 部 分 ， 可 以 看 到 在 
滤波 前 存在 着 由 噪声 产生 的 许多 孤立 的 点 。 经 过 统计 外 点 去 除 之 后 ， 我 
们 消去 了 这 些 噪 声 ， 使 得 整个 地 图 变 得 更 干 当 。 另 一 方面 ， 我 们 在 体 素 
滤波 器 中 ， 把 分 辩 率 调 至 0.01， 表 示 每 立方 厘米 有 一 个 点 。 这 是 一 个 比较 
高 的 分 辨 率 ， 所 以 在 截图 中 我 们 感觉 不 出 地 图 的 差异 ， 然 而 从 程序 输出 
中 可 以 看 到 点 数 明 显 减 少 了 许多 〈 从 90 万 个 点 减 到 了 44 万 个 点 ， 去 除了 
一 半 左 右 ) o 


滤波 前 滤波 后 


图 13-8 ”滤波 前 后 的 对 比 图 (你 可 能 需要 放大 才能 看 清 ， 或 者 自己 在 电脑 上 尝试 一 下 ) 。 

点 云 地 图 为 我 们 提供 了 比较 基本 的 可 视 化 地 图 ， 让 我 们 能 够 大 致 了 
解 环境 的 样子 。 它 以 三 维 方式 存储 ， 使 得 我 们 能 够 快速 地 浏览 场景 的 各 
个 角落 ， 旋 至 在 场景 中 进行 漫游 。 点 云 的 一 大 优势 是 可 以 直接 由 RGB-D 


图 像 高 效 地 生成 ， 不 需要 额外 处 理 。 它 的 滤波 操作 也 非常 直观 ， 且 处 理 
效率 尚 能 接受 。 

不 过 ， 使 用 点 云 表达 地 图 仍然 是 十 分 初级 的 ， 我 们 不 妨 按照 之 前 提 
的 对 地 图 的 需求 ， 看 看 点 云 地 图 是 否 能 满足 。 


1. 定 位 需求 : 取决 于 前 端 视 觉 里 程 计 的 处 理 方式 。 如 果 是 基于 特征 点 
的 视觉 里 程 计 ， 由 于 点 云 中 没有 存储 特征 点 信息 ， 所 以 无 法 用 于 基于 特 
征 点 的 定位 方法 。 如 果 前 端 是 点 云 的 ICP， 那 么 可 以 考虑 将 局 部 点 云 对 全 
局 点 云 进行 ICP 以 估计 位 姿 。 然 而 ， 这 要 求全 局 点 云 具 有 较 好 的 精度 。 在 
我 们 这 种 处 理 点 云 的 方式 中 ， 并 没有 对 点 云 本 身 进行 优化 ， 所 以 是 不 够 
的 。 

2. 导 航 与 避 障 的 需求 : 无 法 直接 用 于 导航 和 避 障 。 纯 粹 的 点 云 无 法 表 
示 “ 是 否 有 障碍 物 ” 的 信息 ， 我 们 也 无 法 在 点 云 中 做 “任意 空间 点 是 否 被 占 
据 ” 这 样 的 查询 ， 而 这 是 导航 和 避 障 的 基本 需要 。 不 过 ， 可 以 在 点 云 基 础 
上 进行 加 工 ， 得 到 更 适合 导航 与 避 障 的 地 图 形式 。 


3. 可 视 化 和 交互 : 具有 基本 的 可 视 化 与 交互 能 力 。 我 们 能 够 看 到 场景 
的 外 观 ， 也 能 在 场景 里 漫游 。 从 可 视 化 角度 来 说 ， 由 于 点 云 只 含有 离散 
的 点 ， 而 没有 物体 表面 信息 (MEA) ， 所 以 不 太 符合 人 们 的 可 视 化 
习惯 。 例 如 ， 扣 云 地 图 的 物体 从 正面 看 和 背面 看 是 一 样 的 ， 而 且 还 能 i 
过 物体 看 到 它 背 后 的 东西 : 这 些 都 不 符合 我 们 日 常 的 经 验 ， 因 为 我 们 没 
有 物体 表面 的 信息 。 


综 上 所 述 ， 我 们 说 点 云 地 图 是 “基础 的 ”或 “初级 的 >”， 是 指 它 更 接近 于 
传感器 读 取 的 原始 数据 。 它 具有 一 些 基本 的 功能 ， 但 通常 用 于 调试 和 基 
本 的 显示 ， 不 便 直 接 用 于 应 用 程序 。 如 果 我 们 希望 地 图 有 更 高 级 的 功 
能 ， 点 云 地 图 是 一 个 不 错 的 出 发 点 。 例 如 ， 针 对 导航 功能 ， 我 们 可 以 从 
点 云 出 发 ， 构 建 占据 网 格 地 图 (Occupancy Grid) ， 以 供 导 航 算 法 查询 某 
点 是 否 可 以 通过 。 再 如 ，SfM 中 常用 的 泊 松 重建 69 方法， 就 能 通过 基本 
的 点 云 重 建物 体 网 格 地 图 ， 得 到 物体 的 表面 信息 。 除 泊 松 重建 外 ，Surfel 
亦 是 一 种 表达 物体 表面 的 方式 ， 以 面 元 作为 地 图 的 基本 单位 ， 能 够 建立 
漂亮 的 可 视 化 地 图 017 。 


图 13-9 显 示 了 泊 松 重建 和 Surfel 的 一 个 样 例 ， 可 以 看 到 它们 的 视觉 效 
果 明 显 优 于 纯粹 点 云 建 图 ， 而 它们 都 可 以 通过 点 云 进 行 构 建 。 大 部 分 由 
点 云 转换 得 到 的 地 图 形式 都 在 PCL 库 中 提供 ， 感 兴趣 的 读者 可 以 进一步 探 


索 PCL 库 内 容 。 而 本 书 作 为 入 门 材料 ， 融 不 详尽 地 介绍 每 一 种 地 图 形式 
了 。 


泊 松 重建 示例 Surfel 重 建 示 例 
213-9 ” 泊 松 重建 与 Surfel 的 示意 图 


13.5.2” 八 又 树 地 图 


下 面 介绍 一 种 在 导航 中 比较 常用 的 ， 本 身 有 较 好 的 压缩 性 能 的 地 图 
形式 : 八 叉 树 地 图 。 

在 点 云 地 图 中 ， 我 们 虽然 有 了 三 维 结构 ， 也 进行 了 体 素 滤波 以 调整 
分 辨 率 ， 但 是 点 云 有 几 个 明显 的 缺陷 : 


:点 云 地 图 通常 规模 很 大 ， 所 以 pcd 文 件 也 会 很 大 。 一 幅 640 像 素 x 480 
像素 的 图 像 ， 会 产生 30 万 个 空间 点 ， 需 要 大 量 的 存储 空间 。 即 使 经 过 一 
些 滤波 后 ，pcd 文 件 也 是 很 大 的 。 而 且 讨 厌 之 处 在 于 ， 它 的 “大 ”并 不 是 必 
需 的 。 点 云 地 图 提供 了 很 多 不 必要 的 细节 。 对 于 地 秘 上 的 裙 锋 、 阴 暗 处 
的 影子 ， 我 们 并 不 特别 关心 这 些 东 西 。 把 它们 放 在 地 图 里 是 在 浪费 空 
间 。 由 于 这 些 空间 的 占用 ， 除 非 我 们 降低 分 辨 率 ， 否 则 在 有 限 的 内 存 
中 ， 无 法 建 模 较 大 的 环境 。 然 而 降低 分 辨 率 会 导致 地 图 质量 下 降 。 有 没 
有 什么 方式 对 地 图 进行 压缩 存储 ， 舍 弃 一 些 重复 的 信息 呢 ? 

“点 云 地 图 无 法 处 理 运 动物 体 。 因 为 我 们 的 做 法 里 只 有 “添加 点 ”， 而 
没有 “ 当 点 消失 时 把 它 移 除 ” 的 做 法 。 而 在 实际 环境 中 ， 运 动物 体 的 普遍 
存在 ， 使 得 点 云 地 图 变 得 不 够 实用 。 

而 我 们 接 来 下 要 介绍 的 ， 就 是 一 种 灵活 的 、 压 缩 的 、 又 能 随时 更 新 
的 地 图 形式 : 八 叉 树 (Octo-map) ral o 


我 们 知道 ， 把 三 维 空 间 建 模 为 许多 个 小 方块 〈 或 体 素 ) 是 一 种 常见 
的 做 法 。 如 果 我 们 把 一 个 小 方块 的 每 个 面 平 均 切 成 两 片 ， 那 么 这 个 小 方 
块 就 会 变 成 同样 大 小 的 八 个 小 方块 。 这 个 步骤 可 以 不 断 地 重复 ， 直 到 最 
后 的 方块 大 小 达到 建 模 的 最 高 精度 。 在 这 个 过 程 中 ， 把 “将 一 个 小 方块 分 
成 同样 大 小 的 八 个 ”这 件 事 ， 看 成 “< 从 一 个 节点 展开 成 八 个 子 节点 *”， 那 
么 ， 整 个 从 最 大 空间 细 分 到 最 小 空间 的 过 程 ， 就 是 一 棵 八 叉 树 (Octo- 
tree) 。 


如 图 13-10 所 示 ， 左 侧 显 示 了 一 个 大 立方 体 不 断 地 均匀 分 成 八 块 ， 直 
到 变 成 最 小 的 方块 为 止 。 于 是 ， 整 个 大 方块 可 以 看 成 是 根 节点 ， 而 最 小 
的 块 可 以 看 作 是 “叶子 记 点 ”。 于 是 ， 在 八 叉 树 中 ， 当 我 们 由 下 一 层 节 扣 
往 上 走 一 层 时 ， 地 图 的 体积 就 能 扩大 为 原来 的 八 倍 。 我 们 不 妨 做 一 点 简 
单 的 计算 : 如 果 叶 子 节 操 的 方块 大 小 为 1 cm? ， 那 么 当 我 们 限制 八 又 树 为 
10 层 时 ， 总 共 能 建 模 的 体积 大 约 为 8 cm =1, 073m? ， 这 足够 建 模 一 间 屋 
子 。 由 于 体积 与 深度 呈 指 数 关 系 ， 所 以 当 我 们 用 更 大 的 深度 时 ， 建 模 的 
体积 会 增长 得 非常 快 。 

读者 可 能 会 疑惑 ， 在 扣 云 的 体 素 滤波 器 中 ， 我 们 不 是 也 限制 了 一 个 
体 素 中 只 有 一 个 点 吗 ? 为 何 我 们 说 点 云 占 空间 ， 而 八 叉 树 比较 节省 空间 
呢 ? 这 是 因为 ， 在 八 叉 树 中 ， 我 们 在 节点 中 存储 它 是 否 被 占据 的 信息 。 
然而 ， 不 同 之 处 在 于 ， 当 某 个 方块 的 所 有 子 节 点 都 被 占据 或 都 不 被 占据 
时 》 就 没 必 要 展开 这 个 节点 o 例 如 》 一 开始 地 图 为 空 A 时 ’ 我 们 就 只 需 
一 个 根 节点 ， 而 不 需要 完整 的 树 。 当 向 地 图 中 添加 信息 时 ， 由 于 实际 的 
物体 经 常 连 在 一 起 ， 空 日 的 地 方 也 会 常常 连 在 一 起 ， 所 以 大 多 数 八 叉 树 
节点 都 无 须 展开 到 叶子 层面 。 所 以 说 ， 八 叉 树 比 点 云 节 省 大 量 的 存储 空 
间 。 


图 13-10” 八 叉 树 示意 图 


前 面 说 八 叉 树 的 节点 存储 了 它 是 否 被 占据 的 信息 。 从 点 云层 面 来 
讲 ， 我 们 自然 可 以 用 0 表示 空 日 ，1 表 示 被 占据 。 这 种 0-1 的 表示 可 以 用 一 
个 比特 来 存储 ， 节 省 空间 ， 不 过 显得 有 些 过 于 简单 了 。 由 于 噪声 的 影 
咱 ， 我 们 可 能 会 看 到 某 个 点 一 会 儿 为 0， 一 会 儿 为 1， 或 者 大 部 分 时 刻 为 
0， 小 部 分 时 刻 为 1， 或 者 除了 “是 ”*“ 否 ”两 种 情况 之 外 ， 还 有 一 个 “未 知 ” 的 
状态 。 能 否 更 精细 地 描述 这 件 事 呢 ? 我 们 会 选择 用 概率 形式 表达 某 节 点 
是 否 被 占据 的 事情 。 比 如 ， 用 一 个 浮 点 数 xE [0, 1] 来 表达 。 这 个 x 一 开始 
取 0.5。 如 果 不 断 观测 到 它 被 占据 ， 那 么 让 这 个 值 不 断 增加 ; RZ, WR 
不 断 观测 到 它 是 空 日 ， 那 束 让 它 不 断 减 小 即 可 。 

通过 这 种 方式 ， 我 们 动态 地 建 模 了 地 图 中 的 障碍 物 信息 。 不 过 ， 现 
在 的 方式 有 一 点 小 问题 : 如 果 让 x 不 断 增加 或 减 小 ， 它 可 能 跑 到 [0, 1] 区 间 
之 外 ， 珊 来 处 理 上 的 不 便 。 所 以 我 们 不 是 直接 用 概率 来 描述 某 节 点 被 占 
据 ， 而 是 用 概率 对 数值 (Log-odds) RR. igy E R 为 概率 对 数值 ，x 
为 0~1 的 概率 ， 那 么 它们 之 间 的 变换 由 logit 变 换 描述 : 


y = logit(x) = log (=) . (13.17) 


(13.18) 


可 以 看 到 ， 当 y M-o 变 到 +o 时 ，x 相应 地 从 0 变 到 了 1。 而 当 y 取 0 
Bt, x 取 到 0.5。 因 此 ， 我 们 不 妨 存储 y 来 表达 节点 是 否 被 占据 。 当 不 断 观 
测 到 “占据 * 时 ， 让 y 增加 一 个 值 ;否则 就 让 y 减 小 一 个 值 。 当 查询 概率 
时 ， 再 用 逆 logit 变 换 ， 将 y 转换 至 概率 即 可 。 用 数学 形式 来 说 ， 设 菜 节 点 
为 n ， 观 测 数据 为 z。 那 么 从 开始 到 t 时 刻 某 节点 的 概率 对 数值 为 L (nlz ，， 
)，t +1 时 刻 为 


L(n|21:441) = £(n|z1-4-1) + L(n|z:). (13.19) 


如 果 写 成 概率 形式 而 不 是 概率 对 数 形式 ， 就 会 有 一 点 复杂 : 


1— Polar) 1 — P(nlaur1) Pn) 
Plnlzr) Plnlzi:7-1) 1- P(n) 


P(n|z1.-7) = |1+ (13.20) 


有 了 对 数 概率 ， 我 们 就 可 以 根据 RGB-D 数 据 更 新 整个 八 叉 树 地 图 
了 。 假 设 我 们 在 RGB-D 图 像 中 观测 到 有 菏 个 像素 市 有 深度 d ， 这 说 明了 一 件 
事 : 我 们 在 深度 值 对 应 的 空间 点 上 观察 到 了 一 个 占据 数据 ， 并 且 ， 从 相 
机 光 心 出 发 到 这 个 点 的 线段 上 ， 应 该 是 没有 物体 的 (否则 会 被 遮挡 ) 。 
利用 这 个 信息 ， 可 以 很 好 地 对 八 叉 树 地 图 进行 更 新 ， 并 且 能 处 理 运 动 的 
结构 。 


13.5.3 SCH: 八 又 树 地 图 


下 面 通 过 程序 演示 一 下 octomap 的 建 图 过 程 。 首 先 ， 请 读者 安装 
octomap 库 : https://github.com/OctoMap/octomap。 Octomap 库 主 要 包含 
octomap 地 图 与 octovis (一 个 可 视 化 程序 ) ， 二 者 都 是 cmake 工 程 。 其 主 
要 依赖 doxygen， 可 通过 如 下 命令 安装 : 


1 | sudo apt-get install doxygen 


我 们 直接 来 演示 如 何 通过 前 面 的 5 张 图 像 生 成 八 叉 树 地 图 ， 然 后 将 它 
画 出 来 。 
kA slambook/ch13/dense_RGBD/octomap_mapping.cpp (片段 ) 


#include 


#include 


<iostream> 


<fstream> 


using namespace std; 


#include 


#include 


#include 


#include 


#include 


<opencv2/core/core.hpp> 
<opencv2/highgui/highgui .hpp> 


<octomap/octomap.h> // for octomap 


<Eigen/Geometry> 
<boost/format.hpp> // for formating strings 


int main( int argc, char** argv ) 


{ 


// 图 像 和 位 姿 读 取 部 分 略 
cout<<" 正 在 将 图 像 转 换 为 0ctomap ..."<<endl; 


// octomap tree 


19 octomap::OcTree tree( 0.05 ); // RH ADH 

20 

21 for ( int i=0; i<5; i++ ) 

2 { 

23 cout<<" 45 He AARP: "<<itt<<end1; 

24 cv::Mat color = colorImgs [i]; 

25 cv::Mat depth = depthImgs [i]; 

26 Eigen: :Isometry3d T = poses [i]; 

27 

28 octomap::Pointcloud cloud; // the point cloud in octomap 

29 

30 for ( int v=0; v<color.rows; vt+ ) 

31 for ( int u=0; u<color.cols; ut+ ) 

32 { 

33 unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度 值 
34 if ( d==0 ) continue; // 为 0 表示 没有 测量 到 

35 if ( d >= 7000 ) continue; // 深度 太 大 时 不 稳定 ， 去 掉 
36 Eigen: :Vector3d point; 

37 point [2] = double(d)/depthScale; 

38 point[0] = (u-cx)*point [2] /fx; 

39 point [1] = (v-cy)*point [2] /fy; 

40 Eigen::Vector3d pointWorld = T*point; 

41 // 将 世界 坐标 系 的 点 放 入 点 云 

42 cloud.push_back( pointWorld[0], pointWorld[1], pointWorld[2] ); 
43 } 

44 

45 // 将 点 云 存 入 八 叉 树 地 图 ， 给 定 原点 ， 这 样 可 以 计算 投射 线 

46 tree.insertPointCloud( cloud, octomap::point3d( T(0,3), T(1,3), T(2,3) ) ); 
47 } 

48 

49 // 更 新 中 间 节 点 的 占据 信息 并 写 入 磁盘 

50 tree,updateInner0ccupancy() ; 

51 cout<<"saving octomap ... "<<endl; 

52 tree.writeBinary( "octomap.bt" ); 

53 return 0; 

54 


我 们 使 用 了 octomap::OcTree 来 构建 整 张 地 图 。 实 际 上 ，octomap 提 供 
了 许多 种 八 叉 树 : 有 带 地 图 的 ， 有 带 占据 信息 的 ， 你 也 可 以 自己 定义 每 


个 节点 需要 携带 哪些 变量 。 简 单 起 见 ， 我 们 使 用 了 不 带 颜 色 信息 的 、 最 
基本 的 / \ 叉 树 地 图 。 


Ocotmap 内 部 提供 了 一 个 点 云 结构 。 它 比 PCL 的 点 云 稍 微 简单 一 些 
只 携带 点 的 空间 位 置信 息 。 我 们 根据 RGB-D 图 像 和 相机 位 姿 信息 ， 先 将 
点 的 坐标 转 至 世界 坐标 ， 然 后 放 入 octomap 的 点 云 ， 最 后 交 给 八 又 树 地 
图 。 之 后 ，octomap 会 根据 之 前 介绍 的 投影 信息 ， 更 新 内 部 的 占据 概率 
最 后 保存 成 压缩 后 的 八 又 树 地 图 。 我 们 把 生成 的 地 图 存 成 octomap.bt 文 
件 。 在 之 前 编译 octovis 时 ， 我 们 实际 上 安装 了 一 个 可 视 化 程序 ， 即 
octovis。 现 在 ， 调 用 它 打 开 地 图 文件 ， 就 能 看 到 地 图 的 实际 样子 了 。 


图 13-11 显 示 了 我 们 构建 的 地 图 结果 。 由 于 我 们 没有 在 地 图 中 加 入 颜 
色 信息 ， 所 以 一 开始 打开 地 图 时 将 是 灰色 的 ， 按 “1” 键 可 以 根据 高 度 信息 
进行 染色 。 读 者 可 以 熟悉 一 下 octovis 的 操作 界面 ， 包 括 地 图 的 查看 、 旋 
Fe. FEMS 


TSE i ask ie a 八 又 树 地 图 (0. 1 米 分 辨 率 ) 
as. 11 eee ee 


FAMA/\ RW REGIA, KEAMER, AF 
我 们 构造 时 使 用 的 默认 深度 是 16 层 ， 所 以 这 里 显示 16 层 即 最 高 分 辩 率 
se rare ago 05 米 。 当 我 们 将 深度 减少 一 层 时 ， 八 叉 树 的 叶 
子 节点 往 上 提 一 层 ， 每 个 小 块 的 边 长 就 增加 一 倍 ， are 1 米 。 可 以 看 
到 ， 我 们 能 够 很 容易 地 调节 地 图 分 辩 率 以 适应 不 同 的 场合 


Octomap 还 有 一 些 可 以 探索 的 地 方 ， 例 如 ， 我 们 可 以 方 便 地 查询 任意 
点 的 占据 概率 ， 以 此 设计 在 地 图 中 进行 导航 的 方法 。 读 者 亦 可 比较 点 
云 地 图 与 八 又 树 地 图 的 文件 大 小 。 上 一 节 生 成 的 点 云 地 图 的 磁盘 文件 大 


约 为 6.9MB， 而 octomap 只 有 56KB， 连 点 云 地 图 的 百 分 之 一 都 不 到 ， 可 以 
有 效 地 建 模 较 大 的 场景 。 


13.6 ” *TSDF 地 图 和 Fusion 系 列 


在 本 讲 的 最 后 ， 我 们 介绍 一 个 与 SLAM 非 常 相似 但 又 有 稍 许 不 同 的 研 
究 方向 : 实时 三 维 重 建 。 本 节 内 容 牵 涉 到 GPU 编程 ， 并 未 提供 参考 例 
子 ， 所 以 作为 可 选 的 阅读 材料 。 


在 前 面 的 地 图 模型 中 ， 以 定位 为 主体 。 地 图 的 拼接 是 作为 后 续 加 工 
步 又 放 在 SLAM 框 染 中 的 。 这 种 框 染 成 为 主流 的 原因 是 ， 定 位 算法 可 以 满 
足 实时 性 的 需求 ， 而 地 图 的 加 工 可 以 在 关键 帧 处 进行 处 理 ， 无 须 实时 响 
应 。 定 位 通 音 是 轻 量 级 的 ， 特 别 是 当 使 用 稀疏 特征 或 稀 下 直 接 法 时 ; 相 
应 地 ， 地 图 的 表达 与 存储 则 是 重量 级 的 。 它 们 的 规模 和 计算 需求 较 大 ， 
不 利于 实时 处 理 。 特 别 是 稠密 地 图 ， 往 往 只 能 在 关键 帧 层面 进行 计算 。 


但 是 ， 现 有 做 法 中 ， 我 们 并 没有 对 稠密 地 图 进行 优化 。 比 如 ， 当 两 
幅 图 像 都 观察 到 同一 把 椅子 时 ， 我 们 只 根据 两 幅 图 像 的 位 姿 把 两 处 的 点 
云 进行 辐 加 ， 生 成 了 地 图 。 由 于 位 姿 估计 通常 是 带 有 误差 的 ， 这 种 直接 
拼接 往往 不 够 准确 ， 比 如 同一 把 椅子 的 点 云 无 法 完美 地 蕉 加 在 一 起 。 这 
时 候 ， 地 图 中 会 出 现 这 把 椅子 的 两 个 重 影 这 种 现象 有 时 候 被 形象 地 
MA FEZ” 0 

这 种 现象 显然 不 是 我 们 想 要 的 ， 我 们 希望 重建 结果 是 光滑 的 、 完 整 
的 ， 符 合 我 们 对 地 图 的 认识 的 。 在 这 种 思想 下 ， 出 现 了 一 种 以 “ 建 图 ”为 
主体 ， 而 定位 居于 次 要 地 位 的 做 法 ， 也 就 是 本 节 要 介绍 的 实时 三 维 重 
建 。 由 于 三 维 重 建 把 重建 准确 地 图 作为 主要 目标 ， 所 以 基本 都 需要 利用 
GPU 进行 加 速 ， 甚 至 需要 非常 高 级 的 GPU 或 多 个 GPU 进行 并 行 加 速 ， 通 
单 需要 较 重 的 计算 设备 。 与 之 相反 ，SLAM 是 朝 轻 量 级 、 小 型 化 方向 发 
展 ， 有 些 方案 甚至 放弃 了 建 图 和 回环 检测 部 分 ， 只 保留 了 视觉 里 程 计 。 
而 实时 重建 则 正在 朝 大 规模 、 大 型 动态 场景 的 重建 方向 发 展 。 

自从 RGB-D 传 感 器 出 现 以 来 ， 利 用 RGB-D 图 像 进行 实时 重建 形成 了 
一 个 重要 的 发 展 方 向 ， 陆 续 产 生 了 Kinect Fusion! Dynamic Fusion"?! 


、 Elastic Fusion"! 、 Fusion4DH23 、 Volumn Deform" 等 成 果 。 其 中 ， 
Kinect Fusion 完 成 了 基本 的 模型 重建 ， 但 仅 限 于 小 型 场景 ; 后 续 的 工作 则 


是 将 它 向 大 型 的 、 运 动 的， 甚至 变形 场景 下 拓展 。 我 们 把 它们 看 成 实时 
重建 一 个 大 类 的 工作 ， 但 由 于 种 类 繁多 ， 不 可 能 详细 讨论 每 一 种 的 工作 
原理 。 图 13-12 展 示 了 一 部 分 重建 结果 ， 可 以 看 到 这 些 建 模 结果 非常 精 
细 ， 比 单纯 拼接 点 云 要 细腻 很 多 。 


我 们 就 以 经 典 的 TSDF 地 图 为 代表 进行 介绍 。TSDF 是 Truncated 
Signed Distance Function 的 缩写 ， 不 妨 译作 截断 符号 距离 函数 BA 
“水 数 ” 称 为 “地 图 ”似乎 不 太 受 当 ， 然 而 在 没有 更 好 的 翻译 之 前 ， 我 们 还 是 
暂时 称 它 为 TSDF 地 图 、TSDF 重 建 等 ， 只 要 不 产生 理解 上 的 偏差 即 可 。 


与 八 叉 树 相似 ，TSDF 地 图 也 是 一 种 网 格式 (或 者 说 ,方块 式 ) 的 地 
图 ， 如 图 13-13 所 示 。 先 选 定 要 建 模 的 三 维 空间 ， 比 如 3x 3x 3m 那么 
大 ， 按 照 一 定 分 辩 率 将 这 个 空间 分 成 许多 小 块 ， 存 储 每 个 小 块 内 部 的 信 
息 。 不 同 的 是 ，TSDF 地 图 整个 存储 在 显存 而 不 是 内 存 中 。 利 用 GPU 的 并 
行 特性 ， 我 们 可 以 并 行 地 对 每 个 体 素 进行 计算 和 更 新 ， 而 不 像 CPU 遍 历 
内 存 区 域 那样 不 得 不 串 行 。 


每 个 TSDF 体 素 内 ， 存 储 了 该 小 块 与 距 其 最 近 的 物体 表面 的 距离 。 如 
果 小 块 在 该 物体 表面 的 前 方 ， 它 就 有 一 个 正 的 值 ; 反之 ， 如 果 该 小 块 位 
于 表面 之 后 ， 那 么 就 为 负 值 。 由 于 物体 表面 通常 是 很 薄 的 一 层 ， 所 以 就 
把 值 太 大 的 和 太 小 的 都 取 成 1 和 - 1， 这 样 就 得 到 了 截断 之 后 的 距离 ， 也 就 
是 所 谓 的 TSDF。 那 么 按照 定义 ，TSDF 为 0 的 地 方 就 是 表面 本 身 或 
者 ， 由 于 数值 误差 的 存在 ，TSDF 由 负 号 变 成 正 号 的 地 方 就 是 表面 本 身 。 
在 图 13-13 的 下 部 ， 我 们 看 到 一 个 类 似 于 人 脸 的 表面 出 现在 了 TSDF 改 变 符 
号 的 地 方 。 


上 (b) _ (c) 


—_ 


~ 


(e) 
图 13-12 ”各 种 实时 三 维 重 建 的 模型 。 (a) Kinect Fusion}; (b) Dynamic Fusion; (c) 
Volumn Deform; (d) Fusion4D; (e) Elastic Fusion。 


dx = dy = dz = 3 [meters] Vx = Vy = Vz = {32, 64, 128, 256, 512} [voxels] 


oA 


相机 观察 到 物体 表面 时 形成 的 截断 距离 值 
图 13-13 ”TSDF 示 意图 。 


TSDF 亦 有 “定位 ”与 “ 建 图 ”两 个 问题 ， 与 SLAM 非 常 相似 ， 不 过 具体 
的 形式 与 本 书 前 面 几 讲 介绍 的 稍 有 不 同 。 在 这 里 ， 定 位 问题 主要 指 如 何 
把 当前 的 RGB-D 图 像 与 GPU 中 的 TSDF 地 图 进行 比较 ， 估 计 相 机 位 姿 。 而 
建 图 问题 就 是 如 何 根据 估计 的 相机 位 姿 ， 对 TSDF 地 图 进行 更 新 。 传 统 做 
法 中 ， 我 们 还 会 对 RGB-D 图 像 进行 一 次 双边 贝 叶 斯 滤波 ， 以 去 除 深 度 图 
中 的 噪声 。 

TSDF 的 定位 类 似 于 前 面 介 绍 的 ICP， 不 过 由 于 GPU 的 并 行 化 ， 我 们 
可 以 对 整 张 深度 图 和 TSDF 地 图 进行 ICP 计 算 ， 而 不 必 像 传统 视觉 里 程 计 
那样 必须 先 计算 特征 点 。 同 时 ， 由 于 TSDF 没 有 颜色 信息 ， 意 味 着 我 们 可 
以 只 使 用 深度 图 ， 不 使 用 彩色 图 ， 就 完成 位 姿 估 计 ， 这 在 一 定 程 度 上 摆 


脱 了 视觉 里 程 计 算法 对 光照 和 纹理 的 依赖 性 ， 使 得 RGB-D 重 建 更 加 稳健 中 
。 男 一 万 面 ， 建 图 部 分 亦 是 一 种 并 行 地 对 TSDF 中 的 数值 进行 更 新 的 过 
程 ， 使 得 所 估计 的 表面 更 加 平滑 可 靠 。 由 于 我 们 并 不 过 多 介绍 GPU 相关 
的 内 容 ， 所 以 具体 的 方法 就 不 展开 细 说 了 ， 请 感 兴趣 的 读者 参阅 相关 文 
献 。 


13.7 “小 结 


本 讲 介绍 了 一 些 常见 的 地 图 类 型 ， 励 其 是 稠密 地 图 形式 。 我 们 看 到 
根据 单 目 或 双 目 可 以 构建 稠密 地 图 ， 而 RGB-D 地 图 则 往往 更 加 容易 、 稳 
定 一 些 。 本 讲 的 地 图 偏重 于 度量 地 图 ， 而 拓扑 地 图 形式 由 于 和 SLAM 人 研究 
差别 比较 大 ， 所 以 没有 详细 地 展开 探讨 。 


习题 

1. 推 导 式 (13.6)。 

2. 把 本 讲 的 稠密 深度 估计 改 成 半 和 稠密， 你 可 以 先 把 梯度 明显 的 地 方 得 
选 出 来 。 


3.* 把 本 讲演 示 的 单 目 稠密 重建 代码 从 正 深度 改 成 逆 深 度 ， 并 添加 仿 
射 变换 。 你 的 实验 效果 是 否 有 改进 ? 


4. 你 能 论证 如 何在 八 叉 树 中 进行 导航 或 路 径 规划 吗 ? 


5. 研 究 文 献 [120]， 探 讨 TSDF 地 图 是 如 何 进行 位 姿 估 计 和 更 新 的 。 它 
和 我 们 之 前 讲 过 的 定位 建 图 算法 有 何 异同 ? 


6.* 人 研究 均匀 -高 斯 混合 滤波 器 的 原理 与 实现 。 


[1] 正式 点 叫 Fragile。 

[2] 反之 ， 如 果 运 动 不 知 道 ， 那 么 极 线 也 无 法 确定 。 

[3] 整体 亮 一 些 可 能 由 环境 光照 变 亮 或 相机 曝光 参数 升 高 导致 。 

[4] 请 注意 ， 笛 密 深 度 估 计 运 行 比较 费时 ， 如 果 你 的 计算 机 比较 老 ， 请 耐心 等 候 一 段 时 间 。 
[5] u,v 的 不 确定 性 取决 于 图 像 的 分 辨 率 。 

[6] 不 过 话说 回来 ， 对 深度 图 就 更 加 依赖 了 。 


第 14 讲 SLAM: 现在 与 未 来 


主要 目标 

1. 了 解 经 典 的 SLAM 实 现 方 案 。 

2. 通 过 实验 ， 比 较 各 种 SLAM 方 案 的 异同 。 
3. 探 讨 SLAM 的 未 来 发 展 方向 。 


前 面 介 绍 了 一 个 SLAM 系 统 中 的 各 个 模块 的 工作 原理 ， 这 是 研究 者 们 
多 年 工作 的 结晶 。 目 前 ， 除 了 这 些 理论 框架 之 外 ， 我 们 也 积累 了 许多 优 
秀 的 开源 SLAM 方 案 。 不 过 ， 由 于 它们 大 部 分 的 实现 都 比较 复杂 ， 不 适合 
作为 初学 者 的 上 手 材 料 ， 所 以 我 们 放 到 了 本 书 的 最 后 加 以 介绍 。 相 信 读 
者 通过 阅读 之 前 的 内 容 ， 应 该 能 明白 其 基本 原理 。 


14.1 ”当前 的 开源 方案 


本 讲 是 全 书 的 总 结 ， 我 们 将 带 着 读者 去 看 看 现 有 的 SLAM 方 案 能 做 到 
怎样 的 程度 。 特 别 地 ， 我 们 重点 关注 那些 提供 开源 实现 的 方案 。 在 SLAM 
研究 领域 ， 能 见 到 开源 方案 是 很 不 容易 的 。 往 往 论文 中 介绍 理论 只 占 20% 
的 内 容 ， 其 他 809%6 都 写 在 代码 中 ， 是 论文 里 没有 提 到 的 。 正 是 这 些 研究 者 
们 的 无 私 奉献 ， 推 动 了 整个 SLAM 行 业 的 快速 前 进 ， 使 后 续 研 究 者 有 了 更 
高 的 起 点 。 在 我 们 开始 做 SLAM 之 前 ， 应 该 对 相似 的 万 案 有 深入 的 了 解 ， 
然后 再 进行 目 己 的 研究 ， 这 样 才 会 更 有 意义 。 

本 讲 的 前 半 部 分 将 带领 读者 参观 一 下 当前 的 视觉 SLAM 方 案 ， 评 述 其 
历史 地 位 和 优 缺 点 。 表 14-1 列 举 了 一 些 常见 的 开源 SLAM 方 案 ， 读 者 可 以 
选择 感 兴趣 的 方案 进行 研究 和 实验 。 限 于 篇 幅 ， 我 们 只 选 了 一 部 分 有 代 
表 性 的 方案 ， 这 肯定 是 不 全 面 的 。 在 后 半 部 分 ， 我 们 将 探讨 未 来 可 能 能 
一 些 发 展 方向 ， 并 给 出 当前 的 一 些 研究 成 果 。 


表 14-1 常 用 开源 SLAM 方 案 


方案 名 称 传感器 形式 | 地 址 


MonoSLAM 单 日 https://github.com/hanmekim/SceneLib2 
PTAM 单 目 http://www.robots.ox.ac.uk/~gk/PTAM/ 
ORB-SLAM 单 目 为 主 http://webdiis.unizar.es/~raulmur/orbslam/ 
LSD-SLAM 单 目 为 主 http://vision.in.tum.de/research/vslam/ 
lsdslam 
SVO 单 目 https://github.com/uzh-rpg/rpg_svo 
DTAM RGB-D https://github.com/anuranbaka/OpenDTAM 
DVO RGB-D https://github.com/tum-vision/dvo_slam 
DSO 单 目 https://github.com/JakobEngel/dso 


RTAB-MAP 双 目 /RGB-D | https://github.com/introlab/rtabmap 
RGBD-SLAM-V2 RGB-D https://github.com/felixendres/rgbdslam_v2 


Elastic Fusion RGB-D https://github.com/mp3guy/ElasticFusion 
Hector SLAM 激光 http://wiki.ros.org/hector_slam 
GMapping 激光 http://wiki.ros.org/gmapping 
OKVIS ZH +IMU | https://github.com/ethz-asl/okvis 
ROVIO 单 目 +IMU | https://github.com/ethz-asl/rovio 


14.1.1 MonoSLAM 


说 到 视觉 SLAM， 很 多 研究 者 第 一 个 想到 的 是 A.J.Davison 的 单 目 
SLAM 工 作 22325 。Davison 教 授 是 视觉 SLAM 人 研究 领域 的 先驱 ， 他 在 2007 年 
提出 的 MonoSLAM 是 第 一 个 实时 的 单 目 视 觉 SLAM 系 统 中 ， 被 认为 是 许多 
工作 的 发 源 地 中。MonoSLAM 以 扩展 卡尔 曼 滤波 为 后 端 ， 追 踪 前 端 非常 
稀 下 的 特征 点 。 由 于 EKF 在 早期 SLAM 中 占据 着 明显 主导 地 位 ， 所 以 
MonoSLAM 亦 是 建立 在 EKF 的 基础 之 上 ， 以 相机 的 当前 状态 和 所 有 路 标 
点 为 状态 量 ， 更 新 其 均值 和 协 方 差 。 


图 14-1 所 示 是 MonoSLAM 在 运行 时 的 情形 。 可 以 看 到 ， 单 目 相 机 在 
一 幅 图 像 当 中 追踪 了 非常 稀疏 的 特征 点 ( 且 用 到 了 主动 追踪 技术 ) 。 在 
EKF 中 ， 每 个 特征 点 的 位 置 服从 高 斯 分 布 ， 所 以 我 们 能 够 以 一 个 椭 球 的 
形式 表达 它 的 均值 和 不 确定 性 。 在 该 图 的 右 半 部 分 ， 我 们 可 以 找到 一 些 
在 空间 中 分 布 着 的 小 球 。 它 们 在 某 个 方向 上 显得 越 长 ， 说 明 在 该 方向 的 
位 置 就 越 不 确定 。 我 们 可 以 想象 ， 如 果 一 个 特征 点 收敛 ， 我 们 应 该 能 


到 它 从 一 个 很 长 的 椭 球 (相机 Z 方 向 上 非常 不 确定 ) 最 后 变 成 一 个 小 点 的 
样子 。 


图 14-1 MonoSLAM 的 运行 时 截图 。 左 侧 : 追踪 特征 点 在 图 像 中 的 表示 ; AM: 特征 点 在 
三 维 空间 中 的 表示 。 


这 种 做 法 在 今天 看 来 固然 存在 许多 浆 端 ， 但 在 当时 已 经 是 里 程 碑 式 
的 工作 了 ， 因 为 在 此 之 前 的 视觉 SLAM 系 统 基 本 不 能 在 线 运行 ， 只 能 靠 机 
器 人 携带 相机 采集 数据 ， 再 离线 地 进行 定位 与 建 图 。 计 算 机 性 能 的 进 
步 ， 以 及 用 称 芷 的 方式 处 理 图 像 ， 加 在 一 起 才 使 得 一 个 SLAM 系 统 能 够 在 
线 地 运行 。 从 现代 的 角度 来 看 ，MonoSLAM 存 在 诸如 应 用 场景 很 窄 ， 路 
标 数量 有 限 ， 稀 疏 特 征 点 非常 容易 丢失 的 情况 ， 对 它 的 开发 也 已 经 停 
止 ， 取 而 代 之 的 是 更 先进 的 理论 和 编程 工具 。 不 过 这 并 不 妨碍 我 们 对 前 
人 工作 的 理解 和 尊敬 。 


14.1.2 PTAM 


2007 年 ，Klein 等 人 提出 了 PTAM (Parallel Tracking and Mapping) |! 
， 这 也 是 视觉 SLAM 发 展 过 程 中 的 重要 事件 。PTAM 的 重要 意义 在 于 以 下 
PARA: 


1.PTAM 提 出 并 实现 了 跟踪 与 建 图 过 程 的 并 行 化 。 我 们 现在 已 然 清 
楚 ， 跟 踪 部 分 需要 实时 响应 图 像 数 据 ， 而 对 地 图 的 优化 则 没 必 要 实时 地 
计算 。 后 端 优化 可 以 在 后 台 慢 慢 进行 ， 然 后 在 必要 的 时 候 进 行 线程 同 步 
即 可 。 这 是 视觉 SLAM 中 首次 区 分 出 前 后 端的 概念 ， 引 领 了 后 来 许多 视觉 
SLAM 系 统 的 设计 (我 们 现在 看 到 的 SLAM 多 半 都 分 前 后 端 ) o 


2.PTAM 是 第 一 个 使 用 非 线 性 优化 ， 而 不 是 使 用 传统 的 滤波 器 作为 后 
端的 方案 。 它 引入 了 关键 帧 机 制 : 我 们 不 必 精 细 地 处 理 每 一 幅 图 像 ， 而 
是 把 几 个 关键 图 像 串 起 来 ， 然 后 优化 其 轨迹 和 地 图 。 早 期 的 SLAM 大 多 数 
使 用 EKF 滤 波 器 或 其 变种 ， 以 及 粒子 滤波 器 等 ; 在 PTAM 之 后 ， 视 觉 
SLAM 研 究 逐 渐 转 向 了 以 非 线 性 优化 为 主导 的 后 端 。 由 于 之 前 人 们 未 认识 
到 后 端 优化 的 稀疏 性 ， 所 以 觉得 优化 后 端 无 法 实时 处 理 那 样 大 规模 的 数 
据 ， 而 PTAM 则 是 一 个 显著 的 反例 。 

PTAM 同 时 是 一 个 增强 现实 软件 ， 演 示 了 酷 炫 的 AR 效果 〈 如 图 14-2 
所 示 ) 。 根 据 PTAM 估 计 的 相机 位 资 ， 我 们 可 以 在 一 个 虚拟 的 平面 上 放置 
虚拟 物体 ， 看 起 来 就 像 在 真实 的 场景 中 一 样 。 


A 


图 14-2 PTAM 的 演示 载 图 。 它 既 可 以 提供 实时 的 定位 和 建 图 ， 也 可 以 在 虚拟 平面 上 姜 加 虚 
拟 物 体 。 


不 过 ， 从 现代 的 眼光 看 来 ，PTAM 也 算是 早期 的 结合 AR 的 SLAM 工 作 
之 一 。 与 许多 早期 工作 相似 ， 存 在 着 明显 的 缺陷 : 场景 小 ， 跟 踪 容 易 丢 


失 ， 等 等 。 这 些 又 在 后 续 的 方案 中 得 以 修正 。 
14.1.3 ORB-SLAM 


介绍 了 历史 上 的 几 种 方案 之 后 ， 我 们 来 看 现代 的 一 些 SLAM 系 统 。 
ORB-SLAM 是 PTAM 的 继承 者 中 非常 有 名 的 一 位 3 ( 见 图 14-3) 。 它 提 
出 于 2015 年 ， 是 现代 SLAM 系 统 中 做 得 非常 完善 、 非 常 易 用 的 系统 之 一 
(如 果 不 是 最 完善 易 用 的 话 ) 。ORB-SLAM 代 表 着 主流 的 特征 点 SLAM 
的 一 个 高 峰 。 相 比 于 之 前 的 工作 ，ORB-SLAM 具 有 以 下 几 条 明显 的 优 
A: 

1. 支 持 单 目 、 双 目 、RGB-D 三 种 模式 。 这 使 得 无 论 我 们 拿 到 了 哪 种 常 
见 的 传感器 ， 都 可 以 先 放 到 ORB-SLAM 上 测试 一 下 ， 它 具有 良好 的 泛 用 
性 。 


2. 整 个 系统 围绕 ORB 特 征 进 行 计算 ， 包 括 视觉 里 程 计 与 回环 检测 的 
ORB 字 典 。 它 体现 出 ORB 特 征 是 现 阶段 计算 平台 的 一 种 优秀 的 效率 与 精 
度 之 间 的 折 中 方式 。ORB 不 像 SIFT 或 SURF 那 样 费 时 ， 在 CPU 上 面 即 可 实 
时 计算 ， 相 比 Harris 角 点 等 简单 角 点 特征 ， 又 具有 良好 的 旋转 和 缩放 不 变 
性 。 并 且 ，ORB 提 供 描 述 子 ， 使 我 们 在 大 范围 运动 时 能 够 进行 回环 检测 
和 重 定 位 。 


3.ORB 的 回环 检测 是 它 的 亮点 。 优 秀 的 回环 检测 算法 保证 了 ORB- 
SLAM 有 效 地 防止 累积 误差 ， 并 且 在 丢失 之 后 还 能 迅速 找 回 ， 这 一 点 许多 
现 有 的 SLAM 系 统 都 不 够 完善 。 为 此 ，ORB-SLAM 在 运行 之 前 必须 加 载 
一 个 很 大 的 ORB 字 上 典 文件 外。 


4.0RB-SLAM 创 新 式 地 使 用 了 三 个 线程 完成 SLAM: 实时 跟踪 特征 点 

的 Tracking 线程 ， 局 部 Bundle Adjustment 的 优化 线程 (Co-visibility 
Graph ， 俗 称 小 图 ) ， 以 及 全 局 Pose Graph 的 回环 检测 与 优化 线程 
(Essential Graph 俗称 大 图 ) 。 其 中 ，Tracking 线 程 负责 对 每 幅 新 来 的 图 
像 提 取 ORB 特 征 点 ， 并 与 最 近 的 关键 帧 进行 比较 ， 计 算 特 征 点 的 位 置 并 
粗略 估计 相机 位 姿 。 小 图 线程 求解 一 个 Bundle Adjustment 问 题 ， 它 包括 局 
部 空间 内 的 特征 点 与 相机 位 姿 。 这 个 线程 负责 求解 更 精细 的 相机 位 次 与 
特征 点 空间 人 位置。 不 过 ， 仅 有 前 两 个 线程 ， 只 完成 了 一 个 比较 好 的 视觉 
里 程 计 。 第 三 个 线程 ， 也 就 是 大 图 线程 ， 对 全 局 的 地 图 与 关键 帧 进行 回 


环 检测 ， 消 除 累 积 误差 。 由 于 全 局 地 图 中 的 地 图 点 太 多 ， 所 以 这 个 线程 
的 优化 不 包括 地 图 点 ， 而 只 有 相机 位 姿 组 成 的 位 姿 图 。 

继 PTAM 的 双 线 程 结 构 之 后 ，ORB-SLAM 的 三 线程 结构 取得 了 非常 好 
的 跟踪 和 建 图 效果 ， 能 够 保证 轨迹 与 地 图 的 全 局 一 致 性 。 这 种 三 线程 结 
构 也 将 被 后 续 的 研究 者 认同 和 采用 。 
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图 14-3”ORB-SLAM 运 行 截图 。 左 侧 为 图 像 与 追踪 到 的 特征 点 ， 右 侧 为 相机 轨迹 与 建 模 的 
特征 点 地 图 。 下 方 为 其 标志 性 的 三 线程 结构 。 


5.ORB-SLAM 围 绕 特征 点 进行 了 不 少 的 优化 。 例 如 ， 在 OpenCV 的 特 
征 提取 基础 上 保证 了 特征 点 的 均匀 分 布 ， 在 优化 位 姿 时 使 用 了 一 种 循环 
优化 4 遍 以 得 到 更 多 正确 匹配 的 方法 ， 比 PTAM 更 为 宽松 的 关键 帧 选取 策 
略 ， 等 等 。 这 些 细小 的 改进 使 得 ORB-SLAM 具 有 远 超 其 他 方案 的 稳健 
性 : 即使 对 于 较 差 的 场景 ， 较 差 的 标定 内 参 ，ORB-SLAM 都 能 够 顺利 地 
工作 。 


上 述 这 些 优势 使 得 ORB-SLAM 在 特征 点 SLAM 中 达到 顶峰 ， 许 多 研 
究 工 作 都 以 ORB-SLAM 作 为 标准 ， 或 者 在 它 的 基础 上 进行 后 续 的 开发 。 
它 的 代码 以 清晰 易 读 著称 ， 有 着 完善 的 注释 ， 可 供 后 来 的 研究 者 进一步 
理解 。 


当然 ，ORB-SLAM 也 存在 一 些 不 足 之 处 。 首 先 ， 由 于 整个 SLAM 系 
统 都 采用 特征 点 进行 计算 ， 我 们 必须 对 每 幅 图 像 都 计算 一 遍 ORB 特 征 ， 
这 是 非常 耗 时 的 。ORB-SLAM 的 三 线程 结构 也 给 CPU 带 来 了 较 重 的 负 
担 ， 使 得 它 只 有 在 当前 PC 架构 的 CPU 上 才能 实时 运算 ,移植 到 同 入 式 设 
备 上 则 有 一 定 困难 。 其 次 ，ORB-SLAM 的 建 图 为 稀疏 特征 点 ， 目 前 还 没 
有 开放 存储 和 读 取 地 图 后 重新 定位 的 功能 (虽然 从 实现 上 来 讲 并 不 困 
YE) 。 根 据 我 们 在 建 图 部 分 的 分 析 ， 稀 疏 特 征 点 地 图 只 能 满足 我 们 对 定 
位 的 需求 ， 而 无 法 提供 导航 、 避 障 、 交 互 等 诸多 功能 。 然 而 ， 如 果 我 们 
仅 用 ORB-SLAM 处 理 定位 问题 ， 似 乎 又 显得 有 些 过 于 重量 级 了 。 相 比 之 
下 ， 另 外 一 些 方案 提供 了 更 为 轻 量 级 的 定位 ， 使 我 们 能 够 在 低 端 的 处 理 
器 上 运行 SLAM, 或 者 让 CPU 有 余力 处 理 其 他 的 事务 。 


14.1.4 LSD-SLAM 


LSD-SLAM (Large Scale Direct monocular SLAM) 是 J .Engle 等 人 于 
2014 年 提出 的 SLAM 工 作 357。 类 比 于 ORB-SLAM 之 于 特征 点 ，LSD- 
SLAM 则 标志 着 单 目 直接 法 在 SLAM 中 的 成 功 应 用 。LSD-SLAM 的 核心 贡 
献 是 将 直接 法 应 用 到 了 半 稠 密 的 单 目 SLAM 中 。 它 不 仅 不 需要 计算 特征 
点 ， 还 能 构建 半 稠 密 的 地 图 一 一 这 里 半 和 稠密 的 意思 主要 是 指 估 计 梯 度 明 
显 的 像素 位 置 。 它 的 主要 优点 如 下 : 


1.LSD-SLAM 的 直接 法 是 针对 像素 进行 的 。 作 者 有 创见 地 提出 了 像素 
梯度 与 直接 法 的 关系 ， 以 及 像素 梯度 与 极 线 方 向 在 稠密 重建 中 的 角度 天 
系 。 这 些 在 本 书 的 第 8 讲 和 第 13 讲 均 有 讨论 。 不 过 ，LSD-SLAM 是 在 单 目 
图 像 进行 半 稠 密 的 跟踪 ， 实 现 原理 要 比 本 书 的 例 程 更 加 复杂 。 


2.LSD-SLAM 在 CPU 上 实现 了 半 稠 密 场景 的 重建 ， 这 在 之 前 的 方案 中 
是 很 少见 到 的 。 基 于 特征 点 的 方法 只 能 是 稀疏 的 ， 而 进行 稠密 重建 的 方 
案 大 多 要 使 用 RGBD 传 感 器 ， 或 者 使 用 GPU 构建 稠密 地 图 "29 。TUM 计 算 
机 视觉 组 在 多 年 对 直接 法 研究 的 基础 上 ， 实 现 了 这 种 CPU 上 的 实时 半 乔 
密 SLAM。 


3. 之 前 也 说 过 ，LSD-SLAM 的 半 稠 密 追 踪 使 用 了 一 些 精妙 的 手段 来 保 
证 追踪 的 实时 性 与 稳定 性 。 例 如 ，LSD-SLAM 既 不 是 利用 单个 像素 ， 也 
不 是 利用 图 像 块 ， 而 是 在 极 线 上 等 距离 取 5 个 点 ， 度 量 其 SSD; 在 深度 估 
计时 ，LSD-SLAM 首 先 用 随机 数 初 始 化 深度 ， 在 估计 完 后 又 把 深度 均值 
归 一 化 ， 以 调整 尺度 ;在 度量 深度 不 确定 性 时 ， 不仅 考 虑 三 角 化 的 几何 
关系， 而 且 考 虑 了 极 线 与 深度 的 夹 角 ， 归 纳 成 一 个 光度 不 确定 性 项 ， 关 
键 帧 之 间 的 约束 使 用 了 相似 变换 群 及 与 之 对 应 的 李 代数 ZE sim(3) 显 式 地 
表达 出 尺度 ， 在 后 端 优化 中 可 以 将 不 同 尺 度 的 场景 考虑 进来 ， 减 小 了 尺 
EABAR 


图 14-4 显 示 了 LSD 的 运行 情况 。 我 们 可 以 观察 一 下 这 种 微妙 的 半 稠 密 
地 图 是 怎样 一 种 介 于 稀疏 地 图 与 稠密 地 图 之 间 的 形式 。 半 稠密 地 图 建 模 
了 灰 度 图 中 有 明显 梯度 的 部 分 ， 显示 在 地 图 中 ， 很 大 一 部 分 都 是 物体 的 
边缘 或 表面 上 带 纹 理 的 部 分 。LSD-SLAM 对 它们 进行 跟踪 并 建立 关键 
帧 ， 最 后 优化 得 到 这 样 的 地 图 。 看 起 来 比 稀疏 的 地 图 具有 更 多 的 信息 ， 
但 又 不 像 稠 密 地 图 那样 拥有 完整 的 表面 (稠密 地 图 一 般 认 为 无 法 仅 用 
CPU 实现 实时 性 ) 。 
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图 14-4 LSD-SLAM 运 行 图 片 。 上 半 部 分 为 估计 的 轨迹 与 地 图 ， 下 半 部 分 为 图 像 中 被 建 模 的 
部 分 ， 即 具有 较 好 的 像素 梯度 的 部 分 。 


由 于 LSD-SLAM 使 用 了 直接 法 进行 跟踪 ， 所 以 它 既 有 直接 法 的 优点 
(对 特征 缺失 区 域 不 敏感 ) ， 也 继承 了 直接 法 的 缺点 。 例 如 ，LSD- 
SLAM 对 相机 内 参 和 曝光 非常 人 敏感， 并且 在 相机 快速 运动 时 容易 丢失 。 另 
外 ， 在 回环 检测 部 分 ， 由 于 目前 并 没有 基于 直接 法 实现 的 回环 检测 方 
式 ， 因 此 LSD-SLAM 必 须 依 赖 于 特征 点 方法 进行 回环 检测 ， 尚 未 完全 摆 
脱 特征 点 的 计算 。 


14.15 SVO 


SVO 是 Semi-direct Visual Odoemtryf 445°". CH Forster AF 
2014 年 提出 的 一 种 基于 稀疏 直接 法 的 视觉 里 程 计 。 按 作者 的 称呼 应 该 叫 
a ae 法 ， 然 而 按照 本 书 的 理念 框架 ， 称 为 “稀疏 直接 法 ”可 能 更 好 一 
些 。 半 直接 在 原文 中 的 意思 是 指 特征 点 与 直接 法 的 混合 使 用 : SVO 跟 踪 
ee 
键 点 周围 的 信息 估计 相机 运动 及 其 位 置 (如 图 14-4 所 示 ) 。 在 实现 中 ， 
SVO 使 用 了 关键 点 周围 的 4x 4 的 小 块 进行 块 匹 配 ， 估 计 相 机 自身 的 运 
动 。 


相 比 于 其 他 方案 ，SVO 的 最 大 优势 是 速度 极 快 。 由 于 使 用 稀疏 的 直 
接 法 ， 它 既 不 必 费 力 去 计算 描述 子 ， 也 不 必 处 理 像 稠密 和 半 笛 密 那 么 多 
的 信息 ， 因 此 ， 即 使 在 低 端 计算 平台 上 也 能 达到 实时 性 ， 而 在 PC 平台 上 
则 可 以 达到 每 秒 100 多 帧 的 速度 。 在 后 续 的 SVO 2.0 中 ， 速 度 更 达到 了 惊 
人 的 每 秒 400 帧 。 这 使 得 SVO 非 常 适 用 于 计算 平台 受 限 的 场合 ， 例 如 无 人 
机 、 手 持 AR/VR 设 备 的 定位 。 无 人 机 也 是 作者 开发 SVO 的 目标 应 用 平 


a 
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14-5 ”SVO 跟 踪 关 键 点 的 图 片 。 


SVO 的 另 一 创新 之 处 是 提出 了 深度 滤波 器 的 概念 ， 并 推导 了 基于 均 
匀 - 高 斯 混合 分 布 的 深度 滤波 器 。 这 在 本 书 的 第 13 讲 有 提 及 ， 但 由 于 原理 
较为 复杂 ， 我 们 没有 详细 解释 。SVO 将 这 种 滤波 器 用 于 关键 点 的 位 置 估 
计 ， 并 使 用 了 逆 深 度 作 为 参数 化 形式 ， 使 之 能 够 更 好 地 计算 特征 点 位 
置 。 


开源 版 的 SVO 代 码 清晰 易 读 ， 十 分 适合 读者 作为 第 一 个 SLAM 实 例 进 
行 分 析 。 不 过 ， 开 源 版 SVO 也 存在 一 些 问题 : 


1. 由 于 目标 应 用 平台 为 无 人 机 的 俯视 相机 ， 其 视野 内 的 物体 主要 是 地 
面 ， 而 且 相 机 的 运动 主要 为 水 平和 上 下 的 移动 ，SVO 的 许多 细节 是 围绕 
这 个 应 用 设计 的 ， 这 使 得 它 在 平视 相机 中 表现 不 佳 。 例 如 ，SVO 在 单 目 
初始 化 时 ， 使 用 了 分 解 太 和夫 阵 而 不 是 传统 的 F 或 已 窍 阵 的 方式 ， 这 需要 假 
设 特征 点 位 于 平面 上 。 该 假设 对 俯视 相机 是 成 立 的 ， 但 对 平视 相机 通常 
是 不 成 立 的 ， 可 能 导致 初始 化 失败 。 再 如 ，SVO 在 关键 帧 选择 时 ， 使 用 
了 平移 量 作为 确定 新 的 关键 帧 的 策略 ， 而 没有 考虑 旋转 量 。 这 同样 在 无 
人 机 俯视 配置 下 是 有 效 的 ， 但 在 平视 相机 中 则 会 容易 丢失 。 所 以 ， 如 果 
读者 想 要 在 平视 相机 中 使 用 SVO， 必 须 自 己 加 以 修改 。 


2.SVO 为 了 速度 和 轻 量 化 ， 人 舍弃 了 后 端 优化 和 回环 检测 部 分 ， 也 基本 
没有 建 图 功能 。 这 意味 着 SVO 的 位 姿 估 计 必 然 存 在 累积 误差 ,而且 丢 失 
后 不 太 容易 进行 重 定位 (因为 没有 描述 子 用 来 回环 检测 ) 。 所 以 ， 我 们 
称 它 为 一 个 VO， 而 不 是 称 它 为 完整 的 SLAM。 


14.1.6 RIAB-MAP 


介绍 了 几 款 单 目 SLAM 方 案 后 ， 我 们 再 来 看 一 些 RGB-D 传 感 器 上 的 
SLAM 方 案 。 相 比 于 单 目 和 双 目 ，RGB-D SLAM 的 原理 要 简单 很 多 (R 
管 实现 上 不 一 定 ) ， 而 且 能 够 在 CPU 上 实时 建立 稠密 的 地 图 。 


RTAB-MAP (Real Time Appearance-Based Mapping) "° 是 RGB-D 
SLAM 中 比较 经 典 的 一 个 方案 。 它 实现 了 RGB-D SLAM 中 所 有 应 该 有 的 
东西 : 基于 特征 的 视觉 里 程 计 、 基 于 词 袋 的 回环 检测 、 后 端的 位 姿 图 优 
化 ， 以 及 点 云 和 三 角 网 格 地 图 。 因 此 ，RTABMAP 给 出 了 一 套 完整 的 (但 
有 些 庞大 的 ) RGB-D SLAM 方 案 。 目 前 我 们 已 经 可 以 直接 从 ROS 中 获得 
其 二 进 制程 序 ， 此 外 ， 在 Google Project Tango 上 也 可 以 获取 其 App 使 用 

(如 图 14-6 所 示 ) o 
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图 14-6 RTAB-MAP%£Google Project Tango 上 的 运行 样 例 。 


RTIAB-MAP 支 持 一 些 常见 的 RGB-D 和 双 目 传感器 ， 像 Kinect、 Xtion 
等 ， 且 提供 实时 的 定位 和 建 图 功能 。 不 过 由 于 集成 度 较 高 ， 使 得 其 他 开 
发 者 在 它 的 基础 上 进行 二 次 开发 变 得 困难 ， 所 以 RTAB-MAP 更 适合 作为 
SLAM 应 用 而 非 研究 使 用 。 


除了 这 些 开 源 方案 之 外 ， 读 者 还 能 在 openslam.org 之 类 的 网 站 上 找到 
许多 其 他 的 研究 ， 例 如 ，DVO-SLAMI27 、RGBD-SLAM-V2al 、DSOG8 
， 以 及 一 些 Kinect Fusion 相 关 的 工作 ， 等 等 。 随 着 时 代 发 展 ， 更 新 弛 、 更 
优秀 的 开源 SLAM 作 品 亦 将 出 现在 人 们 的 视野 中 ， 限 于 篇 幅 这 里 就 不 逐一 


介绍 了 。 


看 过 了 现 有 的 方案 ， 我 们 再 来 讨论 一 些 未 来 的 发 展 方向 a 。 大 体 上 
讲 ，SLAM 将 来 的 发 展 趋 势 有 两 大 类 : 一 是 朝 轻 量 级 、 小 型 化 方向 发 展 ， 
i 上 SLAM 能 够 在 窜 入 式 或 手机 等 小 型 设备 上 良好 运行 ， 然 后 考虑 以 它 为 底 
层 功能 的 应 用 。 毕 竟 ， 大 部 分 场合 中 ， 我 们 的 真正 目的 都 是 实现 机 器 
人 、AR/VR 设 备 的 功能 ， 比 如 说 运动 、 导 航 、 教 学 、 娱 乐 ， 而 SLAM 是 为 
上 层 应 用 提供 自身 的 一 个 位 姿 估 计 。 在 这 些 应 用 中 ， 我 们 不 希望 SLAM 占 
用 所 有 计算 资源 ， 所 以 对 SLAM 的 小 型 化 和 轻 量 化 有 非常 强烈 的 需求 。 另 
一 方面 则 是 利用 高 性 能 计算 设备 ， 实 现 精密 的 三 维 重 建 、 场 景 理 解 等 功 
能 。 在 这 些 应 用 中 ， 我 们 的 目的 是 完美 地 重建 场景 ， 而 对 于 计算 资源 和 
设备 的 便携 性 则 没有 多 大 限制 。 由 于 可 以 利用 GPU ， 这 个 方向 和 深度 学 
习 亦 有 结合 点 。 


14.2.1 ”视觉 + 惯性 导航 SLAM 


首先 ， 我 们 要 谈 一 个 有 很 强 应 用 背景 的 方向 : 视觉 -惯性 导航 融合 
SLAM 方 案 。 实 际 的 机 器 人 也 好 ， 硬 件 设 备 也 好 ， 通 常 都 不 会 只 携带 一 种 
传感器 ， 往 往 是 多 种 传 感 颖 的 融合 。 学 术 界 的 研究 人 员 喜 爱 “ 大 而 且 干 兆 
的 问题 (Big Clean Problem) ， 比 如 说 仅 用 单个 摄像 头 实现 视觉 
SLAM。 但 产业 界 的 朋友 们 则 更 注重 让 算法 更 加 实用 ， 不 得 不 面 对 一 些 复 
杂 而 琐碎 的 场景 。 在 这 种 应 用 背景 下 ， 用 视觉 与 惯性 导航 融合 进行 SLAM 


惯性 传感器 (IMU) 能 够 测量 传感器 本 体 的 角速度 和 加 速度 ， 被 认 
为 与 相机 传感器 具有 明显 的 互补 性 ， 而 且 十 分 有 潜力 在 融合 之 后 得 到 更 
完善 的 SLAM 系 统 028 。 为 什么 这 么 说 呢 ? 


1.IMU 哩 然 可 以 测 得 角速度 和 加 速度 ， 但 这 些 量 都 存在 明显 的 漂移 
(Drift) ， 使 得 积分 两 次 得 到 的 位 次 数据 非常 不 可 靠 。 好 比 说 ,我们 将 
IMU 放 在 桌 上 不 动 ， 用 它 的 读数 积分 得 到 的 位 姿 也 会 漂 出 十 万 八 千里 。 
但 是 ， 对 于 短 时 间 内 的 快速 运动 ，IMU 能 够 提供 一 些 较 好 的 估计 。 这 正 
是 相机 的 弱点 。 

当 运 动 过 快 时 ， (SARJA) 相机 会 出 现 运 动 模糊 ， 或 者 两 帧 之 
间 重 到 区 域 太 少 以 至 于 无 法 进行 特征 匹配 ， 所 以 纯 视觉 SLAM 非 党 害怕 快 
速 的 运动 。 而 有 了 IMU ， 即 使 在 相机 数据 无 效 的 那 段 时 间 内 ， 我 们 也 能 
保持 一 个 较 好 的 位 次 估计 ， 这 是 纯 视觉 SLAM 无 法 做 到 的 。 


2. 相 比 于 IMU ， 相 机 数据 基本 不 会 有 漂移 。 如 果 相 机 放 在 原 地 固定 不 
动 ， 那 么 (在 静态 场景 下 ) 视觉 SLAM 的 位 姿 估 计 也 是 固定 不 动 的 。 所 
以 ， 相 机 数据 可 以 有 效 地 估计 并 修正 IMU 读 数 中 的 漂移 ， 使 得 在 慢 速 运 
动 后 的 位 姿 估 计 依然 有 效 。 

3. 当 图 像 发 生变 化 时 ， 本 质 上 我 们 没 法 知道 是 相机 自身 发 生 了 运动 ， 
还 是 外 界 条 件 发 生 了 变化 ， 所 以 纯 视觉 SLAM 难 以 处 理 动态 的 障碍 物 。 而 
IMU 能 够 感受 到 自己 的 运动 信息 ， 从 某 种 程度 上 减轻 动态 物体 的 影响 。 

总 而 言 之 ， 我 们 看 到 IMU 为 快速 运动 提供 了 较 好 的 解决 方式 ， 而 相 
机 又 能 在 慢 速 运动 下 解决 IMU 的 漂移 问题 一 一 在 这 个 意义 下 ， 它 们 二 者 
是 互补 的 。 


Vl-Sensor DUO 3D 
14-7 ” 越 来 越 多 的 相机 开始 集成 IMU 设 备 。 


当然 ， 昌 然 说 得 很 好 听 ， 不 管 是 理论 还 是 实践 VIO 
(VisualInertialOdometry) 都 是 相当 复杂 的 。 其 复杂 性 主要 来 源 于 IMU 测 
量 加 速度 和 角速度 这 两 个 量 的 事实 ， 所 以 不 得 不 引入 运动 学 计算 。 目 前 
VIO 的 框架 已 经 定型 为 两 大 类 : MHA (Loosely Coupled) MAMA 
(Tightly Coupled) 52 。 松 耦合 是 指 IMU 和 相机 分 别 进行 自身 的 运动 估 
计 ， 然 后 对 其 位 次 估计 结果 进行 融合 。 紧 耦合 是 指 把 IMU 的 状态 与 相机 
的 状态 合并 在 一 起 ， 共 同 构建 运动 方程 和 观测 方程 ， 然 后 进行 状态 估计 
这 和 我 们 之 前 介绍 的 理论 非常 相似 。 我 们 可 以 预见 ， 紧 耦合 理论 也 
必 将 分 为 基于 滤 疲 和 基于 优化 两 个 方向 。 在 滤波 方面 ， 传 统 的 EKF0320 以 
及 改进 的 MSCKF (Multi-State Constraint KF) 5020 都 取得 了 一 定 的 成 果 ， 
研究 者 对 EKF 也 进行 了 深入 的 讨论 (例如 能 观 性 3) ; 优化 方面 亦 有 相 
应 的 方案 131。 值 得 一 提 的 是 ， 尽 管 在 纯 视觉 SLAM 中 优化 方法 已 经 占 


了 主流 ， 但 在 VIO 中 ， 由 于 IMU 的 数据 频率 非常 高 ， 对 状态 进行 优化 需要 
的 计算 量 就 更 大 ， 因 此 目前 仍 处 于 滤波 与 优化 并 存 的 阶段 "% 中 。 由 于 过 
于 复杂 ， 限 于 篇 幅 ， 这 里 就 只 能 大 概 地 介绍 一 下 这 个 方向 了 。 

VIO 为 将 来 SLAM 的 小 型 化 与 低 成 本 化 提供 了 一 个 非常 有 效 的 方向 。 
而 且 结 合 稀 琉 直接 法 ， 我 们 有 望 在 低 端 硬 件 上 取得 良好 的 SLAM 或 VO 效 
果 ， 是 非常 有 前 景 的 。 


14.2.2 语义 SLAM 


SLAM 的 另 一 个 大 方向 就 是 和 深度 学 习 技 术 结 合 。 到 目前 为 止 ， 
SLAM 的 方案 都 处 于 特征 点 或 者 像素 的 层级 。 关 于 这 些 特 征 点 或 像素 到 底 
来 目 于 什么 东西 ， 我 们 一 无 所 知 。 这 使 得 计算 机 视觉 中 的 SLAM 与 我 们 人 
类 的 做 法 不 怎么 相似 ， 至 少 我 们 自己 从 来 看 不 到 特征 点 ， 也 不 会 去 根据 
特征 点 判断 自身 的 运动 方向 。 我 们 看 到 的 是 一 个 个 物体 ， 通 过 左右 眼 判 
断 它 们 的 远近 ， 然 后 基于 它们 在 图 像 当 中 的 运动 推测 相机 的 移动 。 


很 久之 前 ， 研 究 者 就 试图 将 物体 信息 结合 到 SLAM 中 。 例 如 文献 
[135,136,137,138] 中 就 曾 把 物体 识别 与 视觉 SLAM 结 合 起 来 ， 构 建 带 物 体 
标签 的 地 图 。 另 一 方面 ， 把 标签 信息 引入 到 BA 或 优化 端的 目标 函数 和 约 
束 中 ， 我 们 可 以 结合 特征 点 的 位 置 与 标签 信息 进行 优化 0 。 这 些 工作 都 
可 以 称 为 语义 SLAM。 综 合 来 说 ，SLAM 和 语义 的 结合 点 主要 有 两 个 方面 


[9] : 


1. 语 义 帮 助 SLAM。 传 统 的 物体 识别 、 分 割 算法 往往 只 考虑 一 幅 图 ， 
而 在 SLAM 中 我 们 拥有 一 台 移 动 的 相机 。 如 果 我 们 把 运动 过 程 中 的 图 片 都 
带 上 物体 标签 ， 就 能 得 到 一 个 带 有 标签 的 地 图 。 另 外 ， 物 体 信息 亦 可 为 
回环 检测 、BA 优 化 市 来 更 多 的 条 件 。 


2.SLAM 帮 助 语义 。 物 体 识 别 和 分 割 都 需要 大 量 的 训练 数据 。 要 让 分 
类 器 识别 各 个 角度 的 物体 ， 需 要 从 不 同 视角 采集 该 物体 的 数据 ， 然 后 进 
行人 工 标 定 ， 非 常 平 苗 。 而 SLAM 中 ， 由 于 我 们 可 以 估计 相机 的 运动 ， 可 
以 自动 地 计算 物体 在 图 像 中 的 位 置 ， 节 省 人 工 标定 的 成 本 。 如 果 有 自动 
生成 的 带 高 质量 标注 的 样本 数据 ， 能 够 很 大 程度 上 加 速 分 类 器 的 训练 过 


程 。 


图 14-8 语义 SLAM 的 一 些 结果 ， 左 图 和 右 图 分 别 来 自 文献 [138,140]。 


在 深度 学 习 广泛 应 用 之 前 ， 我 们 只 能 利用 支持 向 量 机 、 条 件 随 机 场 
等 传统 工具 对 物体 或 场景 进行 分 割 和 识别 ， 或 者 直接 将 观测 数据 与 数据 
库 中 的 样本 进行 比较 na?  ， 尝 试 构建 语义 地 图 031411%141 。 由 于 这 些 工 
具 本 身 在 分 类 正确 率 上 存在 限制 ， 所 以 效果 也 往往 不 尽 如 人 意 。 随 着 深 
度 学 习 的 发 展 ， 我 们 开始 使 用 网 络 ， 越 来 越 准确 地 对 图 像 进行 识别 、 检 
测 和 分 割 0%15146147.48,149] 。 这 为 构建 准确 的 语义 地 图 打下 了 更 好 的 基础 "50 
。 我 们 正 看 到 ， 逐 渐 开 始 有 学 者 将 神经 网 络 方法 引入 到 SLAM 中 的 物体 识 
别 和 分 割 ， 甚 至 SLAM 本 身 的 位 姿 估 计 与 回环 检测 中 5% 。 昌 然 这 些 
方法 目前 还 没有 成 为 主流 ， 但 将 SLAM 与 深度 学 习 结合 来 处 理 图 像 ， 亦 是 
一 个 很 有 前 景 的 研究 方向 。 


14.2.3 SLAM 的 未 来 


除 此 之 外 ， 基 于 线 / 面 特征 的 SLAM'%%9 、 动 态 场景 下 的 
SLAMtsusauml 、 多 机 器 人 的 SLAMtasrlel ， 等 等 ， 都 是 研究 者 感 兴趣 并 
RAH. AO 的 观点 ， 视 觉 SLAM 经 过 了 三 个 大 时 代 : 提出 问题 、 
寻找 算法 、 完 善 算法 。 而 我 们 目前 正 处 于 第 三 个 时 代 ， 面 对 着 如 何在 已 
有 的 框架 中 进一步 改善 ， 使 视觉 SLAM 系 统 能 够 在 各 种 干扰 的 条 件 下 稳定 
运行 。 这 一 步 需要 许多 研究 者 的 不 懈 努 力 。 

当然 ， 没 有 人 能 够 预测 未 来 ， 我 们 也 说 不 准 会 不 会 突然 有 一 天 ， 整 
个 框架 都 被 新 的 技术 推倒 重 写 。 不 过 即使 是 那样 ， 今 天 我 们 的 付出 仍 将 
是 有 意义 的 。 没 有 今天 的 研究 ， 也 就 不 会 有 将 来 的 发 展 。 最 后 ， 希 望 读 
者 能 在 读 完 本 书 之 后 ， 对 现 有 的 整个 LAM 系 统 有 了 充分 的 认识 。 我 们 也 
期 待 你 能 够 为 SLAM 研 究 做 出 贡献 


习题 
选择 本 讲 提 到 的 任意 一 个 开源 SLAM 系 统 ， 在 你 的 机 器 上 编译 运行 
它 ， amie He, 


2. 你 应 该 已 经 能 够 看 懂 绝 大 多 数 SLAM 相 关 论 文 了 。 拿 起 纸 和 笔 ， 开 
始 你 的 研究 吧 ! 


[1] 这 是 他 博士 期 间 工 作 的 延续 。 他 现在 也 在 致力 于 将 SLAM 小 型 化 、 低 功率 化 。 
[2] 目前 开源 版 ORB-SLAM 使 用 了 文本 格式 的 字典 ， 改 成 二 进 制 格式 字典 之 后 可 以 加 速 不 少 。 
[3] 这 里 有 一 部 分 是 笔者 个 人 的 理解 ， 不 一 定 完 全 正确 。 


附录 A ”高 斯 分 布 的 


这 里 总 结 一 下 常见 的 高 斯 分 布 的 性 质 ， 它 在 本 书 的 很 多 地 方 都 会 
用 到 。 


A.1 高 斯 分 布 
如 果 一 个 随机 变量 x 服从 高 斯 分 布 N (jy,o )， 那 么 它 的 概率 密度 函 


p(x) = — exp (3) l (A.1) 
其 高 维 形 式 为 
1 1 aes 
p(x) = Jay da (3) exp (-ic —p) E (s= w) . (A.2) 
A.2 高 斯 分 布 的 运算 
A.2.1 线 性 运算 


设 两 个 独立 的 高 斯 分 布 : 
x ~ N( fy, &zz), y ~ N (Hy, Syy), 


那么 ， 它 们 的 和 仍 是 高 斯 分 布 : 


x +y ~ N( pz + Hy, Dee + Dyy)- (A.3) 
如 果 以 常数 a 乘 以 x ， 那 么 ax 满足 : 
ax ~ N(aptz, aX zx). (A.4) 


如 果 取 y =Ax , ABAy 满足 : 


y ~ N(Guz, GEzzG"). (A.5) 


A.2.2 乘 积 
设 两 个 高 斯 分 布 的 乘积 满足 p (xy )=N (u, =), ABA: 
yas a 


i a ; (A.6) 


该 公式 可 以 推广 到 任意 多 个 高 斯 分 布 之 乘积 。 
A.2.3 复 合 运算 
同样 考虑 x 和 y ， 若 其 不 独立 ， 则 其 复合 分 布 为 


ren (| || di; (A.7) 
My Lyx yy 


由 条 件 分 布展 开 式 p (xy )=p (xly)p (vy ) 可 以 推出 ， 条 件 概率 P (xly ) 
满足 : 
p (zy) =N (ps + SayDyy (Y — Hy), Ezz — Dry Tyy Dyz) . (A.8) 

A.3 复 合 的 例子 
下 面 举 一 个 和 卡尔 曼 滤 波 器 相关 的 例子 。 考 虑 随机 变量 x~N (u, 
y= Ax+b+w (A.9) 


其 中 A,b ARES SHARES ess, w ARAN, ASW 
的 高 斯 分 布 : w~N (0,R )o 


我 们 来 看 y 的 分 布 。 根 据 前 面 的 介绍 ， 可 以 推出 : 


p(y) = N (Ap, +b, R+ AX, At + R). (A.10) 


这 为 卡尔 曼 滤 波 器 的 预测 部 分 提供 了 理论 基础 。 


附录 B ROSATI 


ROS 是 机 器 人 研究 领域 一 个 广 为 探 讨 的 主题 。 为 了 避免 使 本 书 阅 
读 门槛 太 高 ， 我 们 没有 在 正文 和 例 程 中 提 到 它 。 然 而 近年 来 ，ROS 正 
逐步 在 各 大 高 校 的 学 生 中 间 得 到 推广 ， 渐 渐 为 人 们 所 熟知 和 接受 ， 所 
以 这 里 也 介绍 一 下 ROS， 和 希望 对 读者 能 有 所 帮助 。 

B.1ROS 是 什么 


ROS (Robot Operating System) 是 Willow Garage 公 司 于 2007 年 发 
布 的 一 个 开源 机 器 人 操作 系统 ， 它 为 软件 开发 人 员 开 发 机 器 人 应 用 程 
序 提供 了 许多 优秀 的 工具 和 库 。 同 时 ， 还 有 优秀 的 开发 者 不 断 地 为 它 
贡献 代码 。 本 质 上 ，ROS 并 不 是 一 个 真正 意义 上 的 操作 系统 ， 而 更 像 
是 基于 操作 系统 之 上 的 一 个 软件 包 。 它 提供 了 众多 在 实际 机 器 人 中 可 
能 遇 到 的 算法 : 导航 、 通 信 、 路 径 规划 ， 等 等 。 

ROS 的 版 本 代号 是 按照 字母 顺序 来 编排 的 ， 并 随 着 Ubuntu 系统 发 
布 更 新 。 通 单一 个 ROS 和 版 本 会 支持 两 到 三 个 Ubuntu 系统 版 本 。ROS 从 
Box Turtle 开 始 ， 截 止 到 本 书写 作 时 (2016) ， 已 经 更 新 到 了 Kinetic 
Kame ( 见 图 B-1) 。 同 时 ，ROS 也 已 经 彻底 重 构 ， 推 出 了 实时 性 更 强 
的 2.0 版 本 。 

ROS 支 持 很 多 操作 系统 ， 支 持 最 完善 的 是 Ubuntu 及 其 衍生 版 本 

(Kubuntu, Linux Mint, Ubuntu GNOME 等 ) ， 对 其 他 Linux 发 布 版 
本 、Windows 等 的 支持 也 有 ， 不 过 没有 那么 完善 。 因 此 ， 推 荐 读者 使 
用 Ubuntu 操作 系统 来 进行 开发 和 研究 。 

ROS 支 持 目前 被 广泛 使 用 的 面向 对 象 的 编程 语言 Ct++， 以 及 脚本 

语言 Python。 你 可 以 选择 自己 喜欢 的 语言 进行 开发 。 


HAMO 


AIIIN ‘Box Turtle 
图 B-1 ROS 各 版 本 命名 方式 


B.2ROS 的 特点 


ROS 的 设计 初衷 ， 就 是 使 机 器 人 开发 能 够 像 计 算 机 开发 一 样 ， 屏 
小 底层 硬件 及 其 接口 的 不 一 臻 性， 最终 使 得 软件 可 以 复 用 。 


而 软件 复 用 也 正 是 软件 工程 优美 性 最 集中 的 体现 之 一 ，ROS 能 够 
以 统一 消息 格式 来 使 得 大 家 只 需要 关注 算法 层面 的 设计 ， 而 底层 硬件 
的 根本 目的 是 接收 各 种 各 样 的 消息 ， 如 图 像 、 数 据 等 。 各 个 硬件 厂商 
将 接收 到 的 数据 都 统一 到 ROS 所 规定 的 统一 消息 格式 下 ， 即 可 让 用 户 
方便 地 使 用 各 种 开源 的 机 器 人 相关 算法 。 


在 第 14 讲 中 提 到 的 常见 的 开源 SLAM 方 案 中 ，ORB-SLAM、ORB- 
SLAM2、LSDSLAM、SVO、DVO、RTAB-MAP、RGBD-SLAM- 
V2、Hector SLAM、Gmapping、ROVIO 等 均 有 ROS 版 本 的 开源 代码 ， 
你 可 以 很 方便 地 在 ROS 中 运行 、 调 试 和 修改 它们 。 


在 调试 SLAM 程 序 时 ， 数 据 的 来 源 通常 有 3 种 : 传感器 、 数 据 集 ， 
以 及 bag 文 件 。 若 手头 没有 相应 的 传感器 ， 通 常 就 需要 利用 虚拟 的 数据 
来 跑 SLAM 程 序 。 其 中 ， 最 方便 的 方式 当 属 利用 ROS 下 的 bag 文 件 发 布 
topic， 然 后 SLAM 程 序 就 可 以 监视 topic 发 出 的 数据 ， 就 像 使 用 真实 的 
传感器 采集 数据 一 样 。 后 面 我 们 会 简单 介绍 一 下 如 何 利 用 bag 文 件 来 模 
拟 真 实 的 传感器 数据 。 


B.3 如 何 快速 上 手 ROS 


ROS 有 完善 的 维基 系统 。 因 而 ， 按 照 官网 的 介绍 在 机 器 上 安装 对 
应 版 本 的 ROS: http://wiki.ros.org/ROS/Installation; 然 后， 阅读 ROS 自 
带 的 教学 程序 即 可 。 你 会 学 习 到 ROS 的 基本 概念 、 主 题 的 发 布 和 订 
阅 ， 以 及 用 Python 和 C++ 控 制 它 们 。 如 果 你 觉得 麻烦 ， 也 可 以 使 用 针 
对 ROS 定 制 的 Ubuntu: http://www.aicrobo.com/ubuntu_for_ros.htmlo 


除了 基本 知识 之 外 ， 你 还 可 以 学 到 一 些 ROS 的 常用 工具 ， 例 如 : 


1.rqt。rqt 是 ROS 下 的 一 个 软件 框架 ， 它 以 插件 的 方式 提供 了 各 种 
各 样 方便 好 用 的 GUI (用 户 图 形 界面 ) 。rdt 的 功能 非常 强大 ， 可 以 实 
时 地 查看 ROS 中 流动 的 消息 。 


2.rosbag。rosbag 是 ROS 提 供 的 一 个 非常 好 用 的 录制 及 播放 topic 数 
据 的 工具 。 当 你 想 实 际 跑 一 下 SLAM 程 序 ， 但 轿 于 手头 没有 实际 的 传 
感 器 时 ， 可 以 考虑 使 用 公开 提供 的 bag 文 件 来 进行 图 像 或 者 数据 的 模 
拟 ， 这 种 方式 与 使 用 一 个 真实 的 传感器 在 感觉 上 并 无 不 同 。rosbag 的 
使 用 方式 请 参考 ROS 的 维基 页 面 。 此 外 ， 许 多 公开 数据 集 也 会 提供 bag 
格式 的 数据 文件 。 


3.rviz。rviz 是 ROS 提 供 的 可 视 化 模块 ， 你 可 以 通过 它 实 时 地 查看 
ROS 中 的 图 像 、 点 云 、 地 图 、 规 划 的 路 径 ， 等 等 ， 从 而 更 方便 地 调试 
程序 。 


我 们 相信 ， 机 器 人 的 硬件 层面 和 软件 层面 一 定 都 会 向 着 统一 架构 
的 方向 前 行 ， 而 ROS 正 是 软件 架构 层面 标准 化 一 个 重要 的 里 程 碑 。 其 
中 ，ROS 1.x 在 之 前 被 大 量 用 于 实验 室 的 研究 ， 或 者 公司 产品 demo 的 
研发 阶段 ， 而 ROS2 则 解决 了 ROS 实 时 性 的 问题 ， 未 来 很 有 可 能 被 直接 
用 于 实际 产品 的 研发 ， 为 推进 工业 级 机 器 人 和 服务 机 器 人 的 应 用 做 出 
重要 的 贡献 。 


本 附录 概述 性 地 介绍 了 有 关 ROS 的 历史 、 优 点 ， 以 及 如 何 利用 
ROS 中 的 一 些 可 视 化 工具 来 辅助 SLAM 程 序 开发 等 。 我 们 希望 读者 系 
统 地 学 习 ROS， 并 使 用 ROS 开 发 自己 的 SLAM 程 序 。 
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eaeweomen 技术 凝聚 实力 专业 创新 出 版 


隆重 向 读者 推荐 《视觉 SLAM 十 四 讲 : 从 理论 到 实践 》。 一 方面 这 本 书 是 业界 少 有 的 涵盖 从 基础 理论 到 
代码 实例 ， 系 统 性 讲解 SLAM 的 书 ; 另 一 方面 ， 本 书 的 作者 和 地 平 线 频 有 渊源 ， 高 翔 曾经 是 我 们 的 算法 
实习 生 ， 颜 沁 害 是 自动 驾驶 算法 工程 师 ， 都 是 在 SLAM 和 领域 非常 杰出 的 青年 专家 ， 走 在 技术 实践 的 最 前 沿 。 
在 移动 互联 网 大 潮 之 后 ， 自 动 宇 驶 、 无 人 机 、 服 务 机 器 人 等 人 工 智能 硬件 会 成 为 下 一 个 产业 爆发 点 ， 其 中 
最 关键 的 技术 之 一 就 是 动态 定位 和 环境 建 模 的 SLAM 技 术 。 本 书 是 国内 最 新 、 最 有 价值 的 有 关 SLAM 技 术 
的 书籍 ， 适 合 有 志 于 从 事 机 器 人 技术 的 研究 生 和 工程 师 ， 一 定 会 让 读者 很 有 收获 。 

/分 和 地平线 机 器 人 创始 人 ， 中 国人 工 智能 学 会 秘书 长 


我 在 新 加 坡 和 加 拿 大 给 学 生 讲 视觉 SLAM 的 时 候 常常 觉得 缺乏 一 本 适合 初学 者 的 教材 。 高 翔 博士 的 《 视 
觉 SLAM 十 四 讲 ， 从 理论 到 实践 》 从 最 基础 的 四 元 数 、 李 代数 讲 起 ， 涵 盖 了 卡尔 曼 滤 波 、Bundle 
Adjustment、Pose-Graph 等 高 级 优化 工具 。 书 中 更 有 最 近 十 多 年 成 功 系统 的 概述 ， 从 2003 年 的 
MonoSLAM 直 到 2016 年 的 ORB-SLAM。 通 篇 既 有 清晰 的 理论 叙述 ， 又 辅 以 大 量 示例 程序 ， 是 一 本 非 
常 好 的 视觉 SLAM 教 材 。 

RF ”加 拿 大西 基 弗 雷 泽 大 学 终身 教授 


视觉 SLAM 随 着 近年 增强 现实 、 无 人 驾驶 等 应 用 的 兴起 而 重新 获得 重大 关注 。 视 觉 SLAM 属 于 计算 机 视觉 
和 机 器 人 研究 的 交叉 领域 ， 因 此 涉及 的 基础 知识 广 而 分 散 。 国 内 专门 的 研究 机 构 相对 较 少 ， 因 此 学 生 入 
门 的 门槛 较 高 。 幸 运 的 是 ， 本 书 不 仅 有 深入 浅 出 的 讲解 ， 同 时 注重 理论 和 实战 的 结合 ， 大 大 降低 了 国内 
学 生 和 相关 从 业者 的 进入 门槛 。 因 此 ， 本 书 非常 值得 初学 者 学 习 实 践 。 

刘海 伟 网 易 感知 与 智能 中 心 增强 现实 算法 架构 师 


作者 的 这 本 书 既是 通俗 有 趣 的 高 科技 演义 ， 又 是 足以 指导 研发 实践 的 翔实 教程 ， 对 国内 SLAM 界 而 言 可 
谓 意义 重大 。 我 甚至 发 现 有 不 少 目前 圈 内 的 一 流 人 才 都 是 因为 看 了 本 书 的 早期 章节 才 决 定 进入 这 个 行业 
并 快速 成 长 起 来 的 。 
本 书 所 涵盖 的 知识 面 、 技 术 细节 ， 甚 至 是 某 些 宝贵 的 最 优 实践 经 验 ， 对 国内 刚刚 起 步 的 虚拟 现实 和 增强 
现实 (VRIAR) 、 无 人 机 、 无 人 车 、 机 器 人 等 行业 ， 都 将 产生 深远 影响 ! 

时 驰 (Chris) 博士 _uSens 凌 感 科技 联合 创始 人 /首席 运营 官 
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